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(g)  VALMIN=  0.05  MILLI-IN 322 
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I.  INTRODUCTION 

One  of  the  primary  concerns  with  the  use  of  robotics  to  do  tasks  is  for 
that  robot  to  perform  the  task  in  real  time  while  maintaining  control  over 
the  arm  with  the  minimum  of  interface  with  the  real  world.  Currently 
attempts  are  being  made  to  control  robots  with  elaborate  Artificial 
Intelligence  (Al)  systems  where  the  robot  responds  to  outside  stimuli  and 
uses  complex  algorithms  to  perform  the  next  motion.  A  drawback  to  this 
procedure  is  that  it  requires  a  large  amount  of  data  to  be  correlated  in  a 
short  period  of  time.  In  the  future  as  sensors  become  more  accurate  and 
computers  speed  up  so  that  the  data  can  be  handled  in  a  real  time  fashion 
then  the  approach  of  Al  will  be  effective.  Until  that  time  Robots  will 
require  pre-designed,  pre-determined  responses  to  make  the  required 
movement.  The  accuracy  of  these  motions  will  depend  on  the  techniques 
that  were  used  in  their  construction.  Three  methods  are  currently  being 
utilized  to  teach  the  Robot  to  perform  the  desired  task. 

A.    TEACHING  METHODS 

The  first  two  methods  used  in  teaching  the  Robot  are  Manual  Teaching 

and  Lead-Through  teaching.  Manual  teaching  is  sometimes  called 

teaching-by-showing  or  guiding  and  has  been  used  since  the  early  1950's. 

Probably  the  most  used  method  for  teaching  a  robot  to  move  in  the  desired 

patterns,  it  involves  manipulating  the  robot  arms,  manually,  while  the 

joint  coordinates  are  being  stored  corresponding  to  each  position.  Once 
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stored,  the  program  is  executed  causing  the  robot  to  move  through  the 
joint  vectors  in  the  specified  sequence.  This  method  does  not  require  a 
computer  which  makes  it  good  for  applications  that  are  repetitive  in 
nature  and  fairly  simple  such  as  spot  welding,  painting,  or  handling 
materials. 

Lead-through  teaching,  which  is  similar  to  manual  teaching,  is  the 
simplest  for  programming  continuous  path  (CP)  robot  systems.  One 
method  is  to  grab  the  manipulator  and  lead  it  through  the  entire  range  of 
motion  at  the  desired  speed,  while  recording  the  continuous  position  of 
each  axis.  Due  to  the  size  and  construction  of  some  robots  it  may  be 
necessary  to  have  an  identical  manipulator,  equipped  with  the  position 
sensors,  to  be  led  by  the  operator.  Sometimes  called  a  robot  simulator  or 
a  teaching  arm,  it  is  grasped  by  the  operator  and  led  through  the  range  of 
motion  required  while  the  coordinate  position  of  the  joints  are  being 
sampled  at  a  constant  frequency  and  stored  in  a  computer.  Disadvantages 
to  the  lead-through  teaching  method  includes  1)  requires  a  simulator;  2) 
unintentional  motions  will  be  recorded  and  then  played  back;  3)  since  it 
is  done  manually  high  precision  is  not  possible;  4)  it  is  impossible  to 
obtain  the  exact  required  velocity;  and  5)  large  memory  size  is  required. 

B.    PROGRAMMING 

The  third  method  used  to  teach  robots  to  perform  is  through  the  use  of 

programming.  To  perform  complex  maneuvers,  or  fairly  random 

maneuvers,  robots  need  to  be  under  the  control  of  some  form  of 

programming.  This  is  to  allow  sensors  to  be  positioned  in  the  correct 
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perspective,  or  data  to  be  retrieved.  There  are  several  levels  of 
programming  to  be  considered. 

Robot  level  programming.  At  this  level  the  computer  programming 
language  correlates  the  sensor  data  and  then  specifies  the  robot  motion. 
This  requires  that  the  programmer  be  familiar  with  not  only  the 
programming  language  but  with  techniques  used  in  sensor  guided  motion. 
There  are  several  types  of  robot  level  programming  systems  that  have 
been  around  since  the  early  sixties.  They  include  MHI  (1960-1961),  WAVE 
(1970-1975),  MINI  (1972-1976),  AL  (1974-present),  VAL 
(1975-PRESENT),  AML  (1977-PRESENT),  TEACH  (1975-1978),  PAL 
(1978-PRESENT),  MCL  (1979-PRESENT),  and  many  others  (Lozano-Perez, 
1983,  pp.  821-841;  Fu,  Gonzalas,  and  Lee,  1987,  pp.  450-473). 

Robot  motion  by  taking  the  basic  concept  of  guiding  and  incorporating  a 

decision  matrix  that  is  based  on  sensor  input.    All  of  the  programming 

systems  require  the  use  of  some  form  of  guiding.  Basic  guiding  is  the 

simplest  form  and  takes  a  sequence  of  robot  positions  and  repeats  them 

back  to  the  robot.  Most  of  the  basic  guiding  systems  are  specified  by  a) 

the  way  in  which  the  positions  are  specified,  or  b)  the  method  that  the 

robot  uses  to  get  from  one  position  to  the  next.  The  most  common  way  to 

specify  the  position  is  to  a)  use  a  teach-pendant,  or  b)  move  the  robot 

through  the  desired  sequence  of  motions  manually  or  by  using  some  sort  of 

master-slave  linkage.  When  using  the  teach-pendant  method,  only  a  few 

points  are  used  and  the  motion  is  usually  straight  line  between  two  points 

relative  to  a  coordinate  system.  Positions  in  between  are  interpolated 

based  on  the  coordinate  system  involved.  Extended  guiding  is  based  on 
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using  sensors  to  tell  the  position  of  an  object  and  basing  the  nnotion  of  the 
robot  relative  to  the  coordinate  system  of  that  object.  Off-line  guiding 
incorporates  the  task  model  with  the  robot  model  and  then  simulates  the 
motion  of  the  robot  in  response  to  a  program  or  guiding  input.  This  allows 
for  some  versatility  in  choosing  the  robot  path  and  allows  for  constraints, 
such  as  minimizing  time,  to  be  incorporated. 

Task  level  programming.  The  user  defines  the  task  and  the  robot 
calculates  the  necessary  parameters  to  perform  the  task.  This  makes  the 
robot  entirely  independent  of  the  user,  requiring  no  special  pre-defined 
positions  or  paths  that  depend  on  complex  geometrical  computations  to  be 
supplied  by  the  user.  Task  level  programming  is  accomplished  with  a 
task-planner,  which,  when  given  a  description  of  the  object  to  be 
manipulated,  initial  and  final  states,  and  a  description  of  the  robot 
carrying  out  the  task,  formulates  a  robot-level  program  designed  to 
accomplish  the  task.  Examples  of  task-level  systems  include  HAND-EYE 
[Stanford],  LAMA  [MIT|,  AUTOPASS  [IBM],  RAPT,  and  ROBEX. 

C.    ROBOT  MODEL 

So  far  we  have  made  no  mention  of  the  Robot  or  the  type  of  simulation 

needed  to  accurately  predict  the  intended  motion  of  the  Robot.  Several 

authors  have  defined  the  type  and  classification  (Keren,  1983;  Coiffet, 

1983,  pp.1 1-37)  of  robot  models  that  must  be  considered  when  selecting  a 

Robot  or  simulation.  Most  of  the  Robots  are  classified  by  the  type  of 

coordinate  system  that  the  arms  move  in.  Of  these  there  are  four  main 

classifications: 
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a.  Linear  robot  model  (cartesian),  where  each  of  three  degrees  of 
freedom  move  independently,  and  in  a  linear  fashion.  The  advantages 
to  this  model  are  the  independence  of  the  degrees  of  freedom  reduce 
the  kinematics  equation  to  a  simple  level,  however  the  model  lacks 
mechanical  flexibility.  It  is  a  good  stepping  stone  to  the  more 
advanced  robot  models. 

b.  Cylindrical  coordinate  robot.  Consists  of  a  horizontal  arm  mounted  on 
a  vertical  column  which  is  attached  to  a  rotating  base.  The  horizontal 
arm  is  capable  of  moving  in  and  out  radially,  and  up  and  down  the 
column.  The  main  disadvantage  is  that  the  resolution  of  the 
movement  increases  as  a  function  of  radial  distance. 

c.  Spherical  coordinate  robots.  Similar  to  the  cylindrical  robot  however 
the  arm  is  placed  at  the  end  of  the  vertical  column  and  telescopes  in 
and  out.  Disadvantage  is  that  the  resolution  is  low  in  two  axes 
directions.  The  advantage  is  that  there  is  more  mechanical  flexibility 
and  the  Robot  can  access  areas  below  the  the  base  plane. 

d.  Articulated  or  Revolute  coordinate  system  where  all  three  of  the 
commanded  motions  are  in  terms  of  angles  (radians).  This  Is 
preferable  to  the  others,  but  the  kinematics,  non-linearity  of  the 
servos,  and  time  varying  torques  on  each  arm,  make  it  a  lot  more 
complicated.  The  main  disadvantage  is  that  the  joint  errors 
accumulate  in  the  end  of  the  arm.  The  advantages  are  that  it  is  the 
most  flexible,  and  can  move  at  higher  speeds  than  the  first  three 
models. 
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Figure  1 .1  (a)-(d)  show  examples  of  each  of  the  coordinate  systems 
involved  along  with  skeleton  models  which  interpret  the  physical  motion 
as  a  mathematical  model.  It  is  easy  to  see  that  the  specific  task  a  robot 
is  to  be  used  for  will  be  a  big  factor  in  determining  the  type  of  robot  that 
is  needed. 

The  number  of  degrees  of  freedom  (d.o.f.)  determine  the  ability  of  the 
robot  to  cover  the  design  space.  The  most  common  d.o.f.  are  two,  three, 
six,  and  seven.  The  advantage  of  two  d.o.f.  is  in  the  simplicity  of  motion 
with  the  disadvantage  of  only  being  able  to  move  in  a  planar  or 
2-dimensional  coordinate  space  (2-D).  Three  d.o.f.  give  a  much  more 
desirable  motion  in  that  a  full  three  dimensional  space  can  be  considered. 
Most  applications  that  exist  will  require  as  much  of  3-dimensional  space 
as  possible  to  be  accessible  to  the  tip  of  the  robot.  Six  degrees  of 
freedom  is  the  desired  end  product,  which  consists  of  the  three  degree  of 
freedom  model  with  a  three  degree  of  freedom  manipulator  at  the  end  of 
its  arm.  This  allows  for  a  full  range  of  motion  similar  to  a  human  hand.  A 
lot  of  interest  is  placed  in  seven  d.o.f.  as  it  will  not  only  cover  the  same 
motion  as  six  d.o.f.  but  has  the  added  advantage  of  more  accurately 
representing  the  motion  of  the  human  body  and  allows  the  robot  more 
freedom  in  moving  from  one  position  to  the  next. 

Another  important  aspect  of  the  Robot  model  is  the  kinematic  and 

dynamic  equations  of  motion  used  to  calculate  the  movement  of  the  robot 

and  describe  the  dynamic  behavior  of  the  manipulator.  Several  methods 

are  available,  each  offering  a  different  viewpoint.  They  are: 

a.  Kinematics.  In  the  kinematics  equations  you  are  given  the  angles  of 
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the  arms  with  respect  to  the  coordinate  system  and  get  as  a  result  the 
position  of  the  end  manipulator. 

b.  Inverse  Kinematics.  The  inverse  kinematics  equation  starts  with  the 
position  of  the  end  manipulator  and  results  in  the  calculation  of  the 
angles  of  the  arms. 

c.  Lagrangian.  The  lagrangian  method  is  based  on  the  calculation  of  the 
kinetic  energy  and  the  potential  energy  of  the  manipulator  and  is  more 
analytic  in  nature. 

d.  Newton  Euler  Formulation.    A  numerical  method  using  the  center  of 
mass  theorem  (Newton's)  and  the  kinematic  theorem  (Euler's)  applied 
to  each  link  of  the  robot  manipulator. 

There  are  several  articles  and  textbooks  (Featherstone,  1983,  p.  35; 
Fu,  Gonzolas,  and  Lee,  1987;  Paul,  1981,  pp.  41-118;  Lee,  1981,  pp.  62-68; 
Lee  and  Ziegler,  1984,  pp.  695-696;  Lee  and  Chang,  1986,  pp.  1-4;  Brady, 
1982,  pp.  51-126)  which  go  into  detail  as  to  which  of  the  equation 
techniques  is  the  best  for  the  application  and  which  method  is  easier  and 
quicker  to  use.  Not  only  do  these  methods  apply  to  the  Robot  Model  but 
they  also  apply  to  the  way  that  the  path  of  the  robot  will  be  calculated. 

D.    PATH  APPLICATION 

Movement  of  the  Robot  requires  that  not  only  do  you  know  where  you 

want  to  go  but  that  you  know  where  you  have  been.  Starting  and  ending  a 

series  of  motions  from  a  pre-defined  "home"  position  allow  all  of  the  path 

motions  to  originate  from  a  known  point  with  a  higher  accuracy  than  if 

you  were  to  start  the  motion  at  the  last  known  point.  There  is  ,  however  a 
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disadvantage  in  that  the  arm  may  be  closer  to  the  next  desired  position 
and  there  may  be  a  significant  time  loss  in  moving  "home"  and  then  back  to 
commence  the  next  series  of  movements.  The  end  result  of  the  motion 
must  also  be  considered  when  determining  where  a  path  equation  is  to  go. 
You  need  to  determine  whether  or  not  the  paths  of  the  arms  and  all  of  the 
joints  need  to  be  programmed  or  just  the  tip  of  the  last  or  furthest  arm. 
That  tip  is  referred  to  as  the  endpoint. 

When  trying  to  avoid  objects  it  is  necessary  to  know  where  all  of  the 
arms  are  at  all  instants.  If  the  object  is  within  the  movement  space  then 
it  may  prevent  the  Robot  from  reaching  some  portions  of  the  workspace. 
Having  the  Robot  tip  blindly  go  to  a  region  where  there  is  no  object  does 
not  mean  that  another  part  of  the  Robot  may  not  hit  the  object.  If  some 
object  is  moving  in  the  workspace  and  must  be  avoided  by  the  Robot, 
computation  of  an  acceptable  path  for  the  Robot  arm  may  be  very  difficult. 
It  is  problems  like  these  that  are  driving  researchers  to  Al  techniques  and 
away  from  Robots  that  do  not  "think". 

E     MOTOR  CONTROL 

The  type  of  motor  control  used  can  be  either  open  loop  or  closed  loop. 

In  an  open  loop  control  system  the  accuracy  is  based  on  the  ability  of  the 

arm  sensors  to  match  the  desired  control  input.  This  is  critical  since  as 

the  load  and  arm  position  change  the  torque  required  of  the  motor  changes. 

In  the  closed  loop  system,  position,  velocity,  or  both  are  compared  with 

the  desired  values  and  the  error  is  fedback  to  the  controlling  circuitry  so 

that  the  error  is  reduced  to  minimum.  With  a  highly  accurate  position 
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servo  the  type  of  control  becomes  less  and  less  a  factor.  Another  factor 
that  affects  the  accuracy  of  the  system  is  the  type  of  motor  drive.  The 
most  accurate  are  the  direct  coupled  drives  while  the  least  accurate 
indirectly  drive  the  arms  through  a  system  of  cables,  gears,  chains,  or 
screws.  The  type  of  motor  to  be  controlled  must  also  be  considered. 
Stepping  motors,  hydraulic  actuators,  do  servo-motors,  or  pneumatic 
cylinders  each  present  unique  problems  in  relationship  to  the  overall 
system  and  path  design. 

Other  things  to  be  considered  depend  on  operating  specifications.  Do 
you  want  to  handle  the  problem  in  minimum  time  or  do  you  need  to 
conserve  energy  or  cost?  Is  track  accuracy  important  or  is  the  speed  of 
the  task  important?  unfortunately  there  is  no  single  answer  to  all  of 
these  problems  and  again  the  task  determines  the  design. 

F.    OBJECTIVES 

The  objective  of  this  thesis  is  to  have  a  Robot  arm  follow  a 
predetermined  path.  The  Path  will  be  defined  as  a  set  of  coordinate  points 
to  be  fed  into  a  program  which  simulates  a  Robot  arm.  That  is  a  path  that 
is  previously  calculated  so  that  all  that  needs  to  be  done  is  to  feed  in  the 
control  signals  to  the  Robot  arm. 

Chapter  Two  models  the  Robot  Manipulator  mathematically  without 

regard  to  the  motor  characteristics  and  considering  only  the  physical 

characteristics  of  the  arm  and  the  coordinate  space  that  the  arm  operates 

in.  Due  to  the  simplicity  a  Cartesian  Robot  was  chosen  to  perform  the 

movements.  Chapter  Three  takes  the  motor  and  derives  a  computer 
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simulation  model  to  be  used  in  conjunction  with  the  Robot  Manipulator  of 
Chapter  Two.  Chapter  Four  integrates  the  chapters  into  a  Dynamic 
Simulation  Language  (DSL)  program  and  tests  the  program  in  single  step 
movements  for  proper  operation  and  timing.  Chapter  Five  develops  a  path 
following  routine  which  takes  cartesian  and  parametric  equations  and 
generates  a  list  of  three  tuples  which  are  the  desired  path  for  the 
Cartesian  Robot  to  follow.    Chapter  Six  adjusts  the  earlier  Robot 
simulation,  adding  control  procedures,  allowing  the  path  three  tuples  to  be 
automatically  entered  into  the  simulation  and  demonstrates  the  Robots 
ability  to  follow  the  generated  path.  Linear,  circular,  helical,  and 
sinusoidal  motion  are  examined  for  proper  operation.  Also  error  analysis 
and  criteria  for  proper  operation  are  discussed. 

Chapter  Seven  takes  the  same  path  model  for  the  Cartesian  Robot  and 
translates  to  the  "Articulated"  or  "revolute"  coordinate  system  for  the 
second  model  to  be  run.  Finally  Chapter  Eight  discusses  ideas  and 
directions  to  be  considered  for  future  work. 


25 


Figure  1.1  Cartesian  Robot  Model  and  Skeleton  Structure  (3-P). 
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Figure  1.2    Cylindrical  Robot  Model  and  Skeleton  Structure. 
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Figure  1.3  Spherical  Robot  Model  and  Skeleton  Structure. 
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Figure  1.4    Articulated  Robot  Model  and  Skeleton  Structure. 
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Figure  1.5    Lever  Diagram  for  2-D1mens1onal  Articulated  Robot. 
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II.  MODELING  THE  ROBOT  MANIPUU\TORS 

A.  INTRODUCTION 

Two  Robot  manipulators  are  being  considered.  The  first  model  consists 
of  a  cartesian  robot,  while  the  second  model  consists  of  a  articulated  (or 
revolute)  robot.  Each  mathematical  model  will  be  configured  with  3  degrees 
of  freedom  (d.o.f.)  and  will  be  developed  using  lagrangian  equations  for  both 
the  dynamic  and  static  cases.  Parameters  of  interest  include:  position, 
velocity,  acceleration,  torque,  mass,  load  capabilities,  and  gravity.  The 
lagrangian  relates  the  the  parameters  of  each  joint  with  all  of  the  other 
joints.  Once  the  lagrangian  has  been  found  for  each  of  the  joints,  then  the 
equations  will  be  related  to  the  second  order  model  for  the  idealized  servo 
motor.  The  cartesian  models  are  being  considered  first  due  to  the  simplicity 
of  the  lagrangian  equations  and  because  it  will  be  beneficial  in  understanding 
the  relationships  in  the  articulated  robot. 

B.  CARTESIAN  ROBOT  MODEL 

The  3  d.o.f.  cartesian  robot  that  is  being  considered  is  shown  in  Figure 
1 .1  and  whose  skeletal  structure  is  displayed  in  Figure  2.1 .  The  motion  of 
the  arms  are  in  the  XYZ  coordinate  space  and  each  of  the  arms  moves  in  a 
linear  (prismatic)  fashion  and  there  are  no  revolute  joints.  Because  of  this 
they  can  permit  only  parallel  motion  and  have  no  rotation  vectors.  Using 
Denavit-Hartenberg  Notation  (appendix  A)  (  Lee,  1982,  pp.  86-70;  Fu, 
Gonzolas,  and  Lee,  1987,  pp.  36-41)  on  Figure  2.1  we  have  the  following 
transformation  matrices  described  by  link  parameters  in  Table  2.1. 
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TABLE  2.1 
LINK  PARAMETERS  FOR  3  D.O.F.  CARTESIAN  ROBOT 


Link 
1 
2 
3 


Variable 


'1 


A 


a 

a 

OC 

0 

OC 

0 

OC 

0 

1    0 

0   di 

0    1 

0   0 

0   0 

1    0 

0   0 

0    1 

1    0 

0   0 

0    1 

0   62 

0   0 

1    0 

0   0 

0    1 

1    0 

0   0 

0    1 

0   0 

0   0 

1    d3 

0   0 

0    1 

COS  a 

sin  a 

1 

0 

1 

0 

1 

0 

[eqn  2 J  ] 
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[eqn   2.  3  1 


Computing  ^T^  from  equations  2.1  to  2.3  [appendix  A]  we  get 
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[eqn  2.  4  ] 
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which  is  the  description  of  the  end  of  the  manipulator  with  respect  to  the 
base.  If  the  manipulator  is  referenced  to  a  coordinate  system  by  a 
transformation  C  with  a  tool  described  by  E,  the  position  and  orientation  of 
the  tool  with  respect  to  the  coordinate  reference  is  described  by  X  as 

X  =  Z    0T3    E.  [eqn  2.5] 

C.    DERIVATION  OF  THE  LAGRANGIAN  EQUATION  OF  MOTION 

Lagrangian  formulation  of  manipulator  dynamics  describes  the  model  in 
terms  of  work  and  energy.  These  dynamic  equations  can  be  derived  in  any 
coordinate  system  and  are  easier  to  express  and  use  than  the  Newton-Euler 
formulation  [ASADA].  The  lagrangian  is  defined  as  the  the  difference 
between  the  kinetic  and  potential  energies  of  the  system 

L  =  K  -  P  [eqn  2.6] 

Once  the  lagrangian  has  been  obtained  for  each  of  the  joints  then  the 
dynamics  equation  is  formulated  as 

Fi=  ^^L _aL  i=1...n  leqn  2.71 

dt 

where  qj  are  in  the  coordinates  in  which  the  kinetic  and  potential  energy 
are  expressed,  dq/dt  is  the  velocity,  and  Fj  is  the  torque  or  force, 
depending  on  whether  qj  is  in  a  linear  or  angular  coordinate  system.  The 

lagrangian  of  the  system  is  described  as  L  for  each  degree  of  freedom  n, 
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that  is  to  be  modeled. 

For  the  3  d.o.f.  cartesian  coordinate  system,  expressed  in  cartesian 
coordinates,  all  of  the  joints  are  linear  so  that  the  dynamics  equation  is 
expressed  in  terms  of  forces,  distance  dj,  velocity  ddj/d t,  and 

acceleration    d  ^  dj/dt^.  For  this  problem  the  lengths  of  the  manipulator 
arms  are  identical,  with  the  mass  of  each  arm  located  at  the  end  of  its  link. 

First  Node: 

Ki=  (1/2)  m^    (d6^id\f  [eqn  2.8] 

and  P^  =  -  m^  g  d^  =  0  [eqn  2.9] 

To  relate  the  second  node  to  the  first  it  is  necessary  to  first  write  the 
terms  for  the  position  and  then  taking  the  derivative  find  the  velocity 
components. 

Second  Node: 

Xg  =  d^  Gfx2/dt=    dd^  I  d\ 

yg  =  dg  dy^dX^    ddg/dt  [eqn2„10] 

Zg  =  0  dz2/dt=    0 


where 
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[VgP  =[Gfx2/cft]2  +  [dy2/dt]2+[dz2/Gft]2  [eqn2.11] 

K2=  (1/2)  m^{{d  di/Gf  t)2+  {d  62/ d  X)^}  [eqn  2.12] 

and     P2=  -rriggdg  =  0  [eqn  2.13] 

Third  node: 

X3  =  di  dx2/dt=    dd^/dt 

Va  =  dg  dy^dt=    ddg/dt  [eqn2.14] 

Z3  =  dg  dz^dt=    ddg/dt 

where 

[V3]2  =[dx3/dt]2  +  [dy3/dt]2+[dz3/dt]2  [eqn  2.15] 

K3=  (1/2)mi{(d  d^/d  t)2+(d  dg/d  t)2+(d  d3/d  t)2)  [eqn  2.16] 

and     P3  =  -  m3  g  d3  [eqn  2.1 7] 

Combining  equations  [2.8],  [2.9],  [2.12],  [2.13],  [2.16],  and  [2.17]  into 
equation  [2.6],  and  solving  we  arrive  at  the  lagrangian 

L={1/2}[(  mi+m2+m3)(d  d^/d  t)2  +(  m2+m3 )(  d  dg/d  t)2 

+  (m3)(dd3/dt)2   ]  +  m3gd3  [eqn  2.18] 
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and  calculating  the  dynamics  equation  for  each  node  from  equation  [2.18]  we 
have 

First  node: 

dUd[d6^/dli\  =  (  m^+mg+mg  )(d  d^/d  t)  [eqn  2.19] 

d{  dUdld6^/dX])/d\  =  (  m^+mg+ma  ){d  ^6^/d  ^)  [eqn  2.20] 

auadi  =0  [eqn  2.21] 

we  arrive  at 

F^  =  (  m^+mg+mg  ){d  ^d^/d  t^)  [eqn  2.22] 

Second  node: 

dUdidd^/dt]  =  (  m2+m3  ){d  dg/d  t)  [eqn  2.23] 

d  (  dUdld 6^1  d  \])ld  t  =  (  mg+mg  ){d  ^dg/d  t^ )  [eqn  2.24] 

9  Ua  dg  =  0  [eqn  2.25] 

we  arrive  at 

Fg  =  (  mg+mg  ){d  ^62/d  t^ )  [eqn  2.26] 

Third  Node: 
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dUd[d  6^/d  t]  =  (  m3)(d  dg/d  t )  [eqn  2.27] 

cl{dUd[dd^fd\])/dX  =  (  m3)(d  %/d  t^ )  [eqn  2.28] 

a  Ua  dg  =  m3  g  [eqn  2.29] 

we  arrive  at 

F3  =  (  m2+m3  ){d  ^dg/Gf  t^ )  -  m3  g  [eqn  2.30] 

Equations  for  F^  [2.22],  Fg  [2.26],  and  F3  [2.30],  are  the  lagrangian  dynamic 

equations  that  we  will  relate  to  the  servo  motor  simulation.  The  values 
selected  for  the  simulations  are 


di 

= 

1  unit 

d2 

= 

1  unit 

da 

= 

1  unit 

m, 

s 

.082  oz  +  2mm  +  loa^ 

m2 

= 

.041  oz  +  mm  +  load 

^3 

= 

.041   oz 

g 

^ 

386.4  in/sec2 

mm 

= 

0.186 

load 

= 

0.0 

A  quick  review  of  Figure  2.1  indicates  that  other  variations  are 
available  as  well.  In  the  calculations  we  assumed  that  the  arm  which 
allowed  X  direction  of  motion  actually  supports  and  moves  the  arms  for  Y 
and  Z  movement.  Also  the  arm  which  allowed  Y  motion  supported  the  arm 
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for  Z  movement.  The  choice  of  one  unit  for  each  of  the  arms  allows  for 
scaling  of  the  arms  to  any  length  configuration.  There  are  many  variations 
but  this  is  the  one  chosen  for  the  calculations. 
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Figure  2.1  Skeleton  Orientations  for  the  Cartesian  Robot. 
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COMPUTER  SIMULATION  MODEL 


A.       INTRODUCTION 

The  DC  servo  motor  that  is  being  simulated  has  been  demonstrated 
(Thaler,  1956,  pp.  410-417;  Ozaslan,  1986,  pp.1 4-1 5)  to  have  the  transfer 
function 


where 


G(s)     =  liKv 

V(s)  S(S(JR/KvKt)+1  )(SL/R+1 ) 

9(s)  =  Angular  position  of  the  shaft 

V(s)  =  Applied  d-c  voltage 

Kv  =  Back  emf  constant 

Kt  =  Torque  constant 

J  =  Total  inertia 

R  =  Armature  resistance 

L  =  Armature  inductance 


leqnS.I] 


Since  the  robot  arm  has  a  large  inertia  when  added  to  the  motor  inertia 
this  causes  the  mechanical  pole  of  the  motor  to  become  smaller.  Coupled 
with  the  R/L  term,  which  is  large,  we  can  ignore  the  electrical  pole  and 
the  transfer  function  for  the  motor  and  robot  arm  is  approximately 


V(s) 


Km 


leqn  3.2] 


With  this  simplification  it  is  easy  to  see  that  Figure  3.1  is  the  block 
diagram  of  the  second  order  motor  model.  As  can  be  seen  we  are  using  a 
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velocity  feedback  loop,  an  idealized  deceleration  curve  for  the  motor,  and 
Bang-Bang  control  to  drive  the  system.  Figure  3.2  ties  the  computer 
simulation  model  to  that  of  the  idealized  motor. 

B.       COMPUTER  MODEL 

The  computer  model  uses  position  feedback  while  the  idealized  motor 
uses  velocity  feedback  of  the  motor.  The  input  parameter  to  the  computer 
model  is  the  desired  position  which  drives  the  ideal  motor  with  the 
simulated  velocity,  V.  When  the  E  of  the  computer  model  has  dropped 
below  a  minimum  value  then  the  arm  is  in  the  desired  position.    Since  the 
deceleration  curve  of  an  ideal  motor  is  similar  to  a  parabolic  curve  it  was 
chosen  as  the  curve  to  be  simulated  in  the  DSL  program.  The  following 
equations  describe  the  parameters  outlined  in  Figure  3.2  for  the  computer 
model. 

C"  =  Km  *  Vsat  [eqn  3.3] 

C  =  B  C"  dt  =  Km*Vsat*t  +  C'(0)  [eqn  3.4] 

C  =  B  C  dt  =  (Km*Vsat*t2)/2  +  C(0)  [eqn  3.5] 

where  C(0)  =  0.  Combining  equations  3.4  and  3.5  and  solving  for  C  we  find 
the  relationship 

C=  (C')2/(2*Km*Vsat)  [eqn  3.6] 

41 


Other  relationships  include  ( for  X-arm  only): 


where 


where 


E  =  XPOS  -  C  [eqn  3.7] 

X'  =  -  A*K1  *(ABS[E])1^  [eqn  3.8] 

A  =  (2*Km*VSAT)  [eqn  3.9] 

XPOS  is  the  desired  movement  in  X  direction 
X'  is  the  commanded  velocity 
Km  is  the  gain  constant : 

Km1  =  59.29 

Km2  =  90.25 

Km3  =  77.44 
K1  is  the  curve  scaling  constant  [0.6] 

X'E  =  X'-K*C'  [eqn  3.10] 

V  =  LIMIT(Vsat,-Vsat,K2*X'E)  [eqn  3.11] 


K  is  the  velocity  loop  feedback  gain  [computer  model][1 .0] 

K2  is  the  amplifier  gain  [1 0000.0] 

Vsat  [150  Volts] 

42 


The  value  of  K2  was  chosen  so  that  the  amplifier  saturates  for  small 
signals  with  full  values  of  fon/vard  and  reverse  drive  signals  [±Vsat]. 

C.      SERVOMOTOR 

The  second  order  computer  model  drives  a  closed  loop-servo  for  each 
joint  of  the  robot  arm.  The  motor  used  to  drive  each  of  the  robot  arms  is  a 
permanent  magnet  motor  drive  with  Table  3.1  listing  the  parametric 
characteristics  (Ozaslan,  1986,  pp.  16-30). 

TABLE  3.1 
PARAMETRIC  DATA  FOR  JOINT  SERVO  MOTORS 


Torque  Constant 

Kt 

14.4 

02-in/amp 

Total  Inertia       Jm 

0.033 

oz-in-sec2/rad 

Damping  coefficient 

Bm 

0.04297 

oz-in-sec/rad 

Back  EMF  Constant 

Kv   • 

0.1012 

volts-sec/rad 

Armature  Inductance 

L 

100 

IJL-henries 

Avg  Terminal  Resistance 

R 

0.91 

ohms 

The  same  servo  drive  unit  will  be  used  for  all  of  the  joints.  Since  we  have 
only  linear  motion  at  the  joints  the  effective  inertia  is  only  the  servo 
motor  inertia.  For  the  cartesian  robot  system  there  is  no  reaction  torque 
(no  angular  acceleration),  no  centripetal  forces  (no  angular  velocity),  and 
no  coriolis  forces.  For  each  joint  we  have 

JT0T1  =  J1  +0.5*(M1  -H  M2  +  M3  +  2*MM  +  LOAD)  [eqn  3.12] 

JT0T2  =  J2  +  0.5*(M2  +  LOAD)  [eqn  3.13] 
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JT0T3  =  J3  +  0.5*(M2  +  M3  +  MM  +  LOAD)  [eqn  3.14] 

where  J1 ,  J2,  and  J3  are  the  motor  drive  inertias  listed  in  Table  3.1 .  The 
mass  values  are 

Ml  =  0,082 
M2  =  0.041 
M3  =  0.041 
MM  =  0.186 
LOAD  =  0.0 

Applying  the  known  values  to  the  open  loop  equation  [eqn  3.1]  we  arrived  at 
the  values  for  KM1 ,  KM2,  and  KM3. 

D.       SERVO  MOTOR  SIMULATION  MODEL 

Taking  into  consideration  the  servo  motor  and  computer  model  values 
we  can  mathematically  model  the  servo  motor  with  the  following 
equations. 

CR"  =MTE/JTOT  [eqn  3.15] 

CR'  =  B  CR"  dt  =  MTE/JTOT  *t  +  C'(0)  [eqn  3.16] 

CR  =  B  CR'  dt  =  ((  MTE/JTOT)  *t2)/2  +  C(0)  [eqn  3.17] 

where  C(0)  =  0„  Combining  equations  3.16  and  3,17  and  solving  for  C  we 

find  the  relationship 
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CR  =  (CR')2/(2*  MTE/JTOT)  [eqn  3.18] 


Other  relationships  include  ( for  single  motor  only): 


Vm  =  V-Kv*CR'  [eqn  3.19] 


MP  =  REALPL(0.0,l7R,Vm/R)  [eqn  3.20] 


MT  =  Kt*MP  [eqn  3.21] 


where 


MTE  =  MT  -  Bnn*CR'  -  TL  [eqn  3.22] 

V  is  calculated  in  the  computer  model 

Kv,  Bm,  Kt  are  from  Table  3.1 

TL       is  the  sum  of  centripetal  Torques,  Centrifugal  Torques, 

Reaction  Torques,  and  Gravitational  Torques  which  is 

zero  for  the  cartesian  robot  arm. 
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IV.  DSL  SIMULATION  PROGRAM 

A.  INTRODUCTION 

In  Chapter  Three  the  computer  model  of  a  robot  servo  arm  was 
presented,  while  in  Chapter  Two  a  mathematical  model  was  constructed 
tying  the  three  robot  arms  together  in  a  cartesian  coordinate  system.  This 
chapter  takes  the  results  of  the  previous  chapters  and  generates  a  Dynamic 
Simulation  Language  [DSL]  Program  which  will  be  used  to  model  the  entire 
3  d.o.f.  Cartesian  Robot.  The  input  into  the  DSL  program  will  be  a  path 
described  by  a  three  tuple  of  coordinates.  The  program  simulates  the  robot 
movement  and  the  next  set  of  coordinate  points  are  inputted.  The  output  of 
the  DSL  Program  is  a  listing  of  the  inputted  and  simulated  values  based  on 
the  method  used  to  input  the  values. 

B.  PHILOSOPHY  OF  OPERATION 

The  method  used  to  input  the  data  along  with  the  control  parameters 

of  the  DSL  Program  determine  how  the  robot  will  perform  and  respond  to 

the  entered  coordinate  points.  The  two  methods  that  are  to  be  considered 

are  the  Position  Method  and  the  Velocity  Method.  The  Position  Method  is 

where  the  control  functions  of  the  DSL  Program  are  controlled  by  the 

accuracy  of  the  output  position  and  are  independent  of  time.  The  Velocity 

Method  fixes  the  time  interval  used  to  input  each  of  the  three  tuple 

coordinates  and  since  the  input  is  position  oriented  we  have  in  effect  a 

velocity  control. 
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The  Position  Method  calculates  the  robot  arm  endpoint  until  the 
voltages  for  all  of  the  arms  drops  to  below  a  stipulated  value.  This 
indicates  that  all  of  the  arms  are  within  the  desired  values  of  accuracy. 
The  advantages  of  the  Position  Method  show  up  in  the  accuracy  of  the  final 
position  of  each  move.  Since  the  step  size  is  predetermined  by  the 
programmer  and  the  accuracy  of  each  step  is  less  then  the  step  size  then 
the  overall  error  has  a  definite  maximum  limit.  The  disadvantage  with  the 
Position  Method  is  that  the  values  will  tend  to  be  very  accurate  at  the 
expense  of  time.  The  values  will  oscillate  around  the  desired  points  until 
all  of  the  voltage  values  are  within  a  certain  minimum  value  [VALMIN]  no 
matter  how  long  it  takes. 

The  Velocity  Method  on  the  other  hand  follows  the  same  trajectory 
but  when  the  time  interval  that  was  predetermined  has  been  reached  the 
next  three  tuple  of  coordinate  points  are  entered.  By  selecting  the 
maximum  velocity  available  for  a  given  step,  the  time,  determined  by  the 
DSL  program,  and  desired  length  for  each  step,  determined  by  the  computer 
path  program,  can  be  adjusted  to  maximize  the  performance  at  each  step. 
With  this  adaptability  the  user  can  optimize  the  overall  time  that  it  takes 
to  perform  the  journey  or  the  user  can  wait  at  each  point.  The  advantage 
of  this  method  is  in  the  versatility  in  controlling  with  either  time,  length 
of  travel,  or  a  mixture  of  both.  The  disadvantage  is  in  the  loss  of  accuracy 
at  the  desired  positions  but  with  proper  selection  of  time  and  length  this 
can  be  minimized. 
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C.       EXCITATION  PROPOSALS 

Along  with  the  selection  of  the  method  to  which  the  DSL  Program  is 
organized  there  is  the  type  of  excitation  used  to  input  the  desired  position. 
This  excitation  comes  in  several  forms  of  which  the  three  of  interest  are 
the  STEP,  RAMP,  or  a  function  such  as  the  modified  SINE  wave.  This 
excitation  can  also  be  delayed  in  time  or  be  a  mixture  of  any  of  the 
excitations  as  desired.  Samples  of  the  STEP,  Figures  4.1  (a)-(e),  and  RAMP 
Excitations,  Figures  4.2  (a)-(e)  and  4.3  (a)-(e),  show  differences  in  the 
overall  response  for  a  single  inputted  position  step  of  0.1  units  to  all  three 
coordinates.  The  single  position  change  is  independent  of  the  time  base 
mentioned  earlier  but  will  give  an  idea  of  the  time  required  to  perform 
that  position  change,  and  the  accuracies  involved  in  moving  from  point  to 
point.  Figure  4.4  shows  three  different  excitation  techniques  used  to  apply 
the  movement  excitation  to  the  DSL  Program.  First,  the  STEP  excitation  is 
the  simplest  way  to  apply  the  movement  to  the  DSL  program.  This  does  not 
however  give  a  smooth  linear  response  from  the  simulated  robot  arm.  As 
seen  in  Figure  4.2  (c)-(e)  the  arm  movement  as  simulated  comes  out  fairly 
non-linear  but  is  still  within  our  earlier  assumption  that  the  maximum 
error  for  each  step  is  less  than  the  length  of  the  step  and  looks  to  be  about 
a  tenth  of  the  step  size  in  this  case. 

The  second  and  third  proposed  excitations  are  RAMP  excitations  and 

are  defined  by  DSL  to  be  the  sum  of  two  RAMPs,  the  first  RAMP  starting  at 

time  zero  (DSL  TIME  function),  and  the  second  RAMP  starting  at  time  TV1 

but  negative  in  value  with  the  same  slope  magnitude  as  the  first  RAMP. 

This  gives  a  RAMP  from  time  zero  to  TV1  with  the  magnitude  at  TV1  equal 
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to  the  same  magnitude  as  the  STEP  input.  Two  separate  TV1's  were  chosen 
to  see  what  the  overall  effects  would  be  on  the  simulation  arm  movement. 

Comparing  the  results  of  a  0.1  unit  position  increment,  in  all  three 
coordinate  directions,  when  excited  by  a  STEP,  Figure  4.1  (a),  and  RAMPs, 
Figure  4.2(a)  and  4.3  (a)],  show  that  there  is  an  increase  in  time  for  the 
RAMP  to  complete  its  movement  and  arrive  at  the  desired  endpoint. 
However,  while  the  time  increased  for  the  RAMP  excitation  there  is  a 
reduction  in  the  arms  velocity  as  seen  in  Figures  4.1  (b),  4.2(b),  and  4.3(b), 
and  the  linearity  of  the  arm  motion  between  desired  points  is  much  better. 
RAMP  excitations  provide  much  more  linear  response  than  STEP  excitations 
and  the  longer  TV1  is  then  the  more  accurate  a  RAMP  excitation  is.  For 
applications  where  accuracy  is  desired  it  may  be  more  beneficial  to  use  a 
RAMP  excitation  with  a  large  TV1 .  Use  of  TV1  instead  of  tjsing  a  RAMP  for 
the  entire  step,  reduces  the  complexity  of  the  input  structure  and  attempts 
to  optimize  the  ramp  for  both  short  and  long  steps.  Although  most  of  the 
steps  are  nearly  the  same  length  there  are  times  when  movement  in  one  of 
the  directions  is  short  and  requires  a  faster  ramp  to  complete  it  in  average 
time.  TV1  was  chosen  as  an  attempt  to  keep  the  speed  up  while  sevicing 
both  long  and  short  steps. 

Close  examination  of  the  two  RAMP  excitations  show  that  the  longer 

the  RAMP,  relative  to  the  time  of  completion  desired,  the  more  linear  the 

travel  between  points  becomes  but  the  longer  it  takes  to  get  there  which 

is  due  to  the  slower  velocities  that  the  arms  peak  at.    When  designing  a 

system  for  operation  it  would  be  a  good  idea  to  incorporate  a  RAMP 

technique  to  lower  the  velocities  of  the  arms  and  reduce  torques  and 
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inertias  placed  on  the  motor  units  along  with  the  added  benefit  of  being 
more  accurate  between  the  inputted  steps. 

D.      MULTIPLE  RUNS 

The  DSL  Programming  Language  can  be  modified  to  tie  together  all  of 
the  concepts  that  have  been  discussed  in  this  chapter.  Some  adaptations 
are  necessary  to  simulate  each  of  the  methods.  Appendix  D  contains  a  copy 
of  all  of  the  modifications  used  to  calculate  all  of  the  different  DSL 
simulations  that  were  used  in  this  thesis.  Table  4.1  lists  all  of  the 
modifications  that  were  needed  and  their  location  in  Appendix  D. 

TABLE  4.1 
DSL  MODIFICATIONS  AND  SIMULATIONS 


PROGRAM 

METHOD 

EXCITATION 

LOCATION 

R0BSIM1 

STEP 

APP.  D-1 

ROBSIM2T 

POSITION 

STEP 

APP.  D-3 

RAMP 

APP.  D-3 

R0BSIM2S 

VELOCITY 

STEP 

APP.  D-4 

RAMP 

APP.  D-4 

The  only  difference  between  the  STEP  and  RAMP  excitation  programs 

are  in  the  way  the  data  points  are  applied  to  the  DSL  simulation  within  the 

DSL  program.  Figure  4.4  shows  a  sample  of  the  STEP  and  RAMP  Excitations 

along  with  the  equations  required  to  adapt  the  DSL  program  for  the  Ramp 

Excitation.  Figure  4.5  lists  the  modifications  that  are  needed  to  change 

the  DSL  program  from  the  Position  Method  of  operation  to  the  Velocity 

Method.  Actual  test  runs  will  be  conducted  and  referenced  in  Chapter  Six. 
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E    TEST  RUNS 

To  insure  the  proper  operation  of  the  DSL  program  it  is  necessary  to 
make  a  series  of  single  runs  to  check  on  timing  and  operation  for  steps  of 
various  sizes  and  negative  steps.  Table  4.2  lists  the  DSL  programs  that 
were  used  to  test  the  Robot  Model  DSL  program  and  shows  the  step  sizes 
and  the  time  that  it  took  each  arm  to  complete  the  desired  step.  Figure 
4.6  shows  the  modifications  that  were  made  to  R0BSIM1  to  make  the  test 
runs.     For  each  direction  of  each  step  a  plot  of  velocity  vs.  distance  and  a 
plot  of  distance  vs  time  was  done.  Due  to  the  number  of  plots  that  were 
done  only  a  representative  set  of  plots  are  shown. 

All  of  the  DSL  simulations  should  give  the  same  time  response  the 
averages  for  each  of  the  step  sizes.  R0BSIM2  was  chosen  as  the  base  for 
the  multiple  run  simulations  was  due  to  the  control  statement  and  graph 
ranges. 

As  can  be  seen  from  Table  4.2  there  is  a  distinct  relationship  between 
the  length  of  the  step  size  and  the  time  that  it  takes  the  robot  simulation 
to  reach  that  value.  We  will  use  this  relationship  when  deciding  the  time 
that  will  be  used  in  the  Velocity  Multiple  Run  simulations  to  set  up  the 
time  interval  for  repetitive  data  input  and  control  FINTIME  for  DSL. 
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TABLE  4.2 
DSL  SINGLE  RUN  MODIFICATIONS 


DSL 

STEP  SIZE 

TIME 

no-3-SECS^ 

PROGRAM 

(UNITS) 

^ 

Y 

z 

R0BSIM1 

1.00 

40.5 

30.0 

34.0 

0.50 

28.0 

20.5 

24.0 

0.20 

19.0 

14.0 

15.0 

0.10 

10,2 

08.5 

10.0 

0.05 

09.0 

06.0 

06.5 

0.02 

05.0 

03.0 

04.0 

R0BSIM2 

0.10 

13.0 

09.5 

11.0 

0.05 

09.0 

07.0 

08.5 

0.02 

06.0 

04.0 

05.0 

0.01 

04.0 

03.0 

03.0 

R0BSIM3 

0.02 

06.0 

04.2 

05.0 

0.01 

04.2 

03.0 

03.5 

0.005 

03.0 

02.2 

02.5 

R0BSIM2N 

-0.10 

13.0 

10.0 

10.5 

-0.05 

09.0 

06.5 

07.5 

-0.02 

06.0 

04.5 

05.0 
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Figure  4.1(a)    0.1  STEP  Excited  X,  Y,  and  Z  Step  Response. 
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Figure  4. 1  (b)    0. 1  STEP  Excited  X,  Y,  and  Z  Phase  Plane. 
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Figure  4. 1  (c)    0. 1  STEP  Excited  XY  Plane  Projection. 
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Figure  4. 1  (d)    0. 1  STEP  Excited  K2  Plane  Projection. 
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Figure  4.1(e)    0.1  STEP  Excited  V2  Plane  Projection. 
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Figure  4.2(a)    0.1  RAMP  Excited  X,  Y,  and  2  Step  Response. 
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Figure  4.2(b)    0.1  RAMP  Excited  X,  Y,  and  Z  Phase  Plane. 
[TV1  =0.5*FINTIME). 
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Figure  4.2(c)    0.1  RAMP  Excited  XY  Plane  Projection. 
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Figure  4.2(d)    0.1  RAMP  Excited  XZ  Plane  Projection. 
ITVI  =  0.5*FINTmE). 
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Figure  4.2(e)    0.1  RAMP  Excited  YZ  Plane  Proiection. 
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Figure  4.3(a)    0.1  RAMP  Excited  X,  Y,  and  Z  Step  Response. 
(TV1  3  0.66*FINTIfiE). 
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Figure  4.3(b)    0. 1  RAMP  Excited  X,  Y,  and  Z  Phase  Plane. 
[TVl  =  0.66*FINTiriE). 


66 


20 


— I 1 r- 

40  60  eo 

CX    f10-3  INCHES} 


100 


PLANE  PROJECTIONS  SIMULRTiON 


120 


-    100 


-      80 


60 


-       40 


-      20 


0  io 


120 


Figure  4.3(c)    0.1  RAMP  Excited  XY  Plane  Projection. 
[TV1  =0.66*FINTIME). 
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Figure  4.3(cl)    0  1  RAMP  Excited  X2  Plane  Projection. 
ITVI  =  0.66*FINTIME). 
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Figure  4.3(e)    0. 1  RAMP  Excited  Y2  Plane  Projection. 
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Figure  4.4    Excitation  Examples  of  STEP  and  RAMP  Excitation 
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Figure  4.5    Modifications  to  R0BSIM2  for  Multiple  Runs 
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ROBSIM  1 
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The  only  difference  in  the  three  simulation  programs  are 
the  CONTROL  parameters  to  the  DSL  program  and  how  the 
graphs  were  plotted  on  the  TEK618.  These  programs  were 
used  to  decide  on  the  values  that  would  best  suit  the 
multiple  runs  that  are  to  be  done. 


Figure  4,6     Modifications  to  ROBSIM  1 ,  2,  2n,  and  3 
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V.  COMPUTER  PROGRAM  PATH  DEVELOPMENT 

A.  INTRODUCTION 

Several  techniques  have  been  developed  to  teach  robots  to  perform  the 
desired  tasks  and  movements.  The  most  significant  of  these  are  1 .)  Manual 
teaching,  2.)    Lead-through  teaching,  and  3.)  Programming.    All  of  these 
methods  require  the  operator  to  manually  lead  the  robot  through  the  desired 
task  and  then  playback  the  resulting  stored  data  or  to  develop  sophisticated 
task  level  programs  that  interface  with  the  real  world  using  position  and 
velocity  sensors.  As  stated  earlier  the  intent  of  this  thesis  is  to  develop  a 
method  of  predetermining  the  control  signals  for  each  of  the  robot  arms 
with  an  accuracy  that  eliminates  the  need  for  elaborate  sensing  equipment. 
Using  a  highly  accurate  positioning  servo  control,  signals  can  be  applied 
based  on  position  alone  which  allows  the  operator  to  precalculate  the  entire 
path  that  the  robot  arm  is  to  follow. 

B.  MANUAL  TEACHING  and  LEAD-THROUGH  TEACHING 

Sometimes  called  teaching-by-showing  or  guiding  involves  manipulating 

the  robot  arms  through  the  desired  patterns,  storing  the  resulting  time  and 

position  coordinates  in  memory,  and  playing  back  the  coordinates  in  the 

desired  sequence.  The  Manual  Teaching  method  does  not  require  a  computer 

to  control  the  movements  and  is  used  mainly  in  repetitive  tasks  such  as 

welding,  painting,  or  material  handling.  The  Lead-Through  requires  a 

computer  or  simulator  to  control.    Disadvantages  to  both  methods  include 
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large  memory  requirements,  unintentional  motions  will  be  recorded  as  the 
arm  is  manually  lead  through  the  task,  high  precision  is  not  possible,  and 
the  exact  velocity  of  the  arm  cannot  be  controlled. 

C.  PROGRAMMING  TECHNIQUES 

Programming  of  complex  or  random  maneuvers  allows  the  robot  arm  to 
be  accurately  positioned.  Robot  level  programming  correlates  the  robots 
sensor  data  and  then  specifies  the  desired  movement.  Motion  by  the  robots 
arm  can  be  predetermined  by  either  of  the  teach  methods  described  earlier 
and  uses  sensors  to  tell  the  position  of  an  object  and  bases  the  motion  of 
the  robot  relative  to  the  coordinate  system  of  the  object.  With  Task  level 
programming  the  user  describes  the  task  and  the  task-planner  formulates  a 
robot  level  program  to  carry  out  that  task.  Either  of  the  level  programming 
techniques  require  a  special  language  to  be  used  in  controlling  the  robot. 

D.  COMPUTER  PATH  DEVELOPMENT 

instead  of  developing  the  coordinates  for  the  robot  movement  by  the 

teach  methods  we  are  precalculating  the  required  robot  control  signals  and 

then  applying  these  control  signals  to  each  of  the  arms  in  an  independent  but 

simultaneous  manner.  First  the  desired  path  that  the  robot  is  to  follow  is 

described  and  then  these  values  are  adjusted  for  the  type  of  robot  arm 

positioning  syncros  being  used  that  were  described  in  Chapter  Three. 

Finally,  in  Chapter  Six,  the  desired  control  signals  are  applied  to  the 

simulated  model  and  the  motion  of  the  simulator  is  compared  to  the  desired 

path. 
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1.     Desired  Path 

Options  in  picking  the  desired  path  have  been  limited  to  make  the 
program  manageable.  The  following  is  a  list  of  the  assumptions  and 
restrictions  in  picking  the  path: 

1 .  Paths  are  of  the  form  Y  =  f(X)  and  Z  =  f(X,Y). 

2.  Start  and  finish  points  are  inputted  as  two-tuples  of  (X,Y).  Start  and 
finish  points  in  the  Z-direction  are  calculated  from  the  equations  that 
are  implemented  in  the  program.  Motion  in  just  one  direction  can  not 
be  accomplished  in  the  cartesian  coordinate  system  because  of  the 
difficulty  in  describing  the  motion.  A  parametric  routine,  added  to 
make  conic  section  equations  easier  to  program,  will  be  able  to  easily 
handle  these  types  of  equations. 

3.  All  values  must  result  in  positive  values  that  will  be  weighted  as 
desired  by  the  user.  This  weight  factor  is  a  linearization  so  that  the 
path  coordinates  are  within  the  range  of  the  robot.  This  is  termed 
scaling  and  will  be  discussed  further  in  Chapter  Six. 

4.  Maximum  travel  lengths  are  based  on  the  maximum  velocity  of  the 
separate  arms  and  by  the  differences  in  each  of  the  arm  torques.  The 
current  programs  assume  that  the  velocities  are  equal  to  the 
maximum  step  length  divided  by  a  fixed  delta  T  and  are  scaled  so  that 
the  maximum  step  sizes  are  all  equal  in  all  three  coordinate 
directions. 

5.  The  user  has  a  choice  of  going  home  after  each  movement  or  going  to 

the  start  point  of  the  next  equation.  If  the  start  of  the  next  equation 

Is  the  same  as  the  finish  of  the  previous  you  must  do  a  FSTAR 
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subroutine  to  align  the  index  variables.  This  inserts  a  repeated  step 
where  the  robot  goes  nowhere  and  does  not  increase  delay  time 
significantly.  The  cost  to  the  DSL  program  is  just  a  few  instructions 
before  the  program  goes  and  gets  the  next  point. 

6.  Three  equation  capability  was  chosen  for  path  description  in  this 
system  for  simplicity,  yet  being  sufficiently  long  enough  to 
demonstrate  the  required  path  structure  (R0BPATH1).  A  two  equation 
capability  will  be  discussed  as  the  primary  program  later  in  this 
chapter  (R0BPATH4). 

7.  The  program  has  been  limited  to  1000  points  of  (X,Y,Z)  coordinate 
points  and  a  maximum  distance  for  the  each  movement  has  been 
chosen  to  be  0.10  units  to  allow  each  of  the  points  to  be  discernible. 
To  observe  smoothness  a  maximum  distance  for  movement  will  be 
reduced  to  0.01  or  0.001  depending  on  the  application.  These  will  be 
demonstrated  further  in  Chapter  Six. 

8.  Movement  in  a  plane  parallel  to  the  axis  has  been  accounted  for. 

9.  Due  to  the  method  of  scaling  the  start  and  finish  points  of  each  arm 
must  be  different  and  distinct  relative  to  the  X  coordinate  axis.  That 
is  the  start  and  finish  points  must  be  in  the  direction  of  the  slope  of 
the  desired  curve.  This  determines  the  direction  that  the  X  increment 
is  taking  so  that  when  applied  to  y=f(x)  we  get  a  legitimate  y 
increment. 
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2.     Cartesian  Robot  Motion 

Since  the  robot  of  concern  here  is  a  cartesian  robot  the  allowable 

motion  has  been  restricted  to  the  first  octant  where  all  of  the  (X,Y,Z) 

coordinate  points  are  positive  and  the  coordinate  system  is  right-handed. 

This  means  that  the  program  will  not  be  sufficient  if  used  with 

non-cartesian  robots  without  some  modifications  or  transformations. 

Transformations  will  be  discussed  further  in  Chapter  Seven.  Figure  5.1 

gives  a  description  of  the  areas  that  are  required  to  be  modified  for 

successful  run  of  the  program.  Figure  5.2  flowcharts  the  input/change 

procedure  required  to  make  the  program  function.  The  full  program  listing 

in  both  FORTRAN  [WF77]  and  Microsoft  BASIC  are  located  in  Appendix  E  and 

F.  Figures  5.3  to  5.7  flowchart  the  program  main  and  the  four  subroutines 

used  in  the  R0BPATH4. FORTRAN  program.  Flowcharts  for  the  BASIC 

program  are  given  in  Appendix  F.  From  now  on  we  will  refer  to  the 

ROBPATH4. FORTRAN  listing  in  all  explanations. 

To  set  up  the  program  for  defining  the  desired  path,  or  series  of 

sub-paths,  of  the  robot  tip  (sometimes  called  the  endpoint)  follow  the 

procedure  set  up  in  Figure  5.2  and  as  seen  in  example  on  Figure  5.1 .    The 

user  must  first  decide  on  the  motion  that  he  wants  the  robot  tip  to  make 

following  the  restrictions  stated  earlier.  Once  the  path  has  been  chosen 

then  the  user  can  modify  the  FORTRAN  program  so  that  the  desired 

calculations  will  follow  the  path.  There  are  four  main  subroutines  which 

help  in  formulating  the  desired  path  movement.  They  are: 

1.  HSTAR:  Describes  motion  of  the  Robot  Arm  from  Home  to  the  Start  of 

the  next  desired  path.  Motion  is  linear  between  Home  and  Start  points. 
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2.  STARF:  Describes  the  motion  of  a  path  from  the  Start  to  Finish  of 
the  desired  sub-path.  This  is  the  program  that  calculates  a  set  of 
three  tuples  of  position  points  based  on  user  inputted  equations  In 
Cartesian  Coordinates. 

3.  FSTAR:  Describes  the  motion  of  a  path  from  the  Finish  of  one 
sub-path  to  the  Start  of  the  next  sub-path.  Motion  is  linear  in  this 
subroutine. 

4.  FHOME:  Describes  the  motion  of  a  path  from  the  Finish  of  a  sub-path 
to  Home.  Motion  is  linear  in  this  subroutine. 

5.  SFPAR:  Describes  the  motion  of  a  path  from  Start  to  Finish  of  a  set 
of  X,  Y,  and  Z  parametric  equations  which  are  a  function  of  a  single 
variable.  The  variable  can  represent  radian  movement,  time 
movement,  or  can  represent  a  user  defined  variable.  Start  and  Finish 
points  are  user  supplied,  positive,  with  the  start  point  [TS]  having  a 
smaller  magnitude  then  the  finish  [TF].  User  must  take  this  into 
consideration  when  writing  the  parametric  equations. 

All  of  the  subroutines  are  similar  in  construction  except  STARF  and 

SFPAR.  They  assume  that  the  path  between  the  start  and  finish  points  are 

linear  and  use  these  points  to  scale  the  all  of  the  incremental  values  to 

within  the  maximum  distance  that  each  coordinate  can  move.  With  the 

subroutine  STARF  the  main  assumption  is  that  changes  in  X  [DELX] 

determine  the  value  of  the  change  in  Y  [DELY]  and  that  these  in  turn 

determine  the  incremental  change  in  Z  [DELZ].  First  the  value  of  the 

difference  in  the  X  start  and  finish  directions  are  used  to  determine  the 

direction  of  the  x  increment.  Then  this  value  is  applied  to  the  first 
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equation  [EQAT1]  to  determine  the  corresponding  y  increment.  If  this 
increment  is  greater  than  the  maximum  value  allowed  in  this  direction 
[MAXODY]  then  the  x  increment  is  linearly  scaled  using  the  following 
relationship: 

DELX  =  DELX  (MAXODY/ABS(DELY)).  [eqn  5.1] 

This  scaling  is  repeated  until  both  values  are  within  their  prescribed 

maximum  values.  Then  the  x  and  y  increments  are  used  to  calculate  the  z 

increment  [DELZ]  using  the  second  equation  [EQUAT1].  If  the  z  increment  is 

larger  than  the  maximum  allowed  z  increment  [MAXODZ]  then  all  three 

increments  are  scaled  to  make  the  z  increment  equal  to  MAXODZ.  Then  all 

three  increments  are  added  to  the  previously  calculated  three  tuple  of 

coordinate  positions  [HSPOS  array]  and  the  process  is  repeated  for  the  next 

set  of  points.  Each  time  a  point  is  generated  there  is  a  check  to  see  if  the 

path  (or  sub-path)  has  reached  the  FINISH  point.  If  the  FINISH  point  has 

been  reached  then  the  last  position  value  is  set  equal  to  the  FINISH  point 

and  the  program  continues  on  to  the  next  subroutine.  The  program  also 

checks  for  plane  movement  and  if  plane  movement  is  noted  then  the  value 

of  the  previous  data  point  is  retained. 

As  each  of  the  subroutines  are  called  the  HSPOS  array  maintaining 

the  data  is  read  into  the  program  and  then  rewritten.    These  sets  of  data 

points  are  to  be  used  in  driving  the  DSL  Simulation  program  [chosen  in 

Chapter  Four]  and  to  compare  the  final  results.  The  main  array  is  HSPOS 

which  is  composed  of  a  the  total  number  of  three  tuples  in  the  array 
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followed  by  columns  containing  the  three  tuple  of  coordinate  positions 
which  will  then  be  used  by  one  of  the  DSL  simulation  programs. 

Chapter  Six  takes  the  results  of  the  FORTRAN  program,  which  are  the 
desired  path  coordinates,  and  compares  each  point  with  the  results  out  of 
the  DSL  program.  It  will  also  contain  any  modifications  to  the  programs  if 
they  are  needed,  such  as  scaling. 


USER  MODIFICATION  REGION 
FUNCTION  MODIFICATION 

EQUATION  1-4  (EQAT  1-4 /EQUAT1-4) 

PARAMETRIC  X/Y/Z  EQUATION  1-4 
MAIN  PROGRAM 

DELT 

MAXOD 

ACCURACY  (IF  USED) (ACC) 

MAX  VELOCITY  CONSTANTS  (CURRENTLY  1) 

DELT  (AGAIN) 

VALUE  MAXIMUM  (VALMAX) 

VALUE  MIINIMUM  (VALMIN) 
EQUATIONS  BOUNDS  AREA 

START  AND  FINISH  POINTS 

OUTPUT  S/F  POINTS 
PATH  MOVEMENT  REGION 

HOME  (DIFFERENT  FOR  DIFFERENT  ARM 
CONFIGURATIONS  AND  TYPE) 

PATH  FORMATION 
PRINTOUT  REGION 

ROBOT  SIMULATION  NUMBER 


CHECKING  THESE  PARAMETERS  ALONG  WITH  FIGURE  5.2 
WILL  GIVE  YOU  THE  PATH.   AFTER  UPDATING  THE 
FILDEFS  NEEDED  AND  COMPILING  IT  IS  RECOMMENDED 
THAT  A  THREE  DIMENSION  PLOT  BE  DONE  TO  INSURE 
THAT  THE  PATH  IS  AS  DESIRED. 


Figure  5. 1    Path  Program  Modification  Regions. 
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WRITE-UP  DESIRED 
PATH  FOR  ROBOT 


Akl 


MODIFY  FUNCTION  BLOCKS 
FOR  *  OF  EQUATIONS 
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SET  UP  PATH  EQUATION 

WITH  START  AND  FINISH 

PTs  (XSJS,ZS,XF,YF,ZF) 


Akl 


DECIDE  ON: 

VALMIN/VALMAX 

MAXOD/ACC 

MAXVELOCITY 


\J^ 


SET  UP  PATH  MOVEMENT 
IN  DESIRED  SEQUENCE 


J^ 


ADAPT  SUBROUTINES  FOR 

MORE/LESS  THAN  THREE 

EQUATIONS  IF  NEEDED 


\J^ 


ADJUST  DIMENSIONS  AND 

INTERNAL  LOOPS 

AS  NEEDED 


\>^ 


CHANGE  OPEN  STATEMENT 

IF  WANT  TO  SAVE  OLD 

PATHS 


\/ 


COMPILE  AND  RUN 


Figure  5.2    Procedure  for  Updating  Path  Equation. 
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r  START  ^ 


:^L^ 

USER  MODIFICATION  AREA 

FUNCTIONS: 

EQUATl     EQUAT2 

EQAT1      EQAT2 


J^ 


USER  PARAMETER  ADJUSTMENTS 
AND  HOUSEKEEPING 


JiJ^ 


EQUATION  PATH  FORMATION 
START/FINISH  POINTS 


:^ 


HOME  POSITION 
DEFINED 


\/L 


INITIALIZE  FIRST  PATH 
MOVEMENT 


JiJ/ 


CALL  HSTAR 


\/ 


ADJUST  FOR  DIFFERENT 
POINT  DENSITY 


\/l. 


CALL  STARF 


^^ 


INITIALIZE  MOVE  BETWEEN 
FIRST  AND  SECOND  EQUATIONS 


Figure  5.3  Robot  Path  Formation  Flowchart.    R0BPATH4.F0RTRAN 
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CALL  FSTAR 


INITIALIZE  FOR  SECOND 
EQUATION 


CALL  STARF 


IE 


INITAIALIZEFOR 
FINAL  MOVEMENT 


\^ 


CALL  FHOME 


\J^ 


OUTPUT  START/FINISH, 
POINTS 


IE 


FORMAT  DATA  FOR  DSL 

PROGRAM  USING  TEMP 

DISK  STORAGE 


OUTPUT  THREE-TUPLE 
OF  POSITION  POINTS 


:^ 


COMPLETED  PATH  PROGRAM 


(^STOP) 


Figure  53(Cont)    Robot  Path  Formation  Flowchart.  R0BPATH4.F0RTRAN 
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f  START  ^ 


\]^ 

PRINT  "HOME  TO  START" 
AND  START/FINISH  POINTS 


:±L 


INPUT  THREE  TUPLE  OF  PTs 
ALREADY  CALCULATED 


(TooF) — ) 


\/ 


ADJUST  MAXIMUM  STEP  SIZE 


CHECK 

FOR  START  POINTS 

AT  ZERO 

NO 


YES 


¥ 


LOOP 
BYPASS 


SCALE  MOVEMENT  INCREMENT 
FOR  MAX   ALLOWED     IN  THREE  DIM 


^J^ 


UPDATE  NEW 
POSITION 


:J^ 


OUTPUT  NEW  POSITION 


7 


YES 


\|        LOOP 
"^     BYPASS 


Figure  5.4  Subroutine  HSTAR.     Home  to  Start. 
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LOOP 
BVPASS 


f 


\/ 


YES 


^ 


J^ 


LAST  POSITION 
UPDATED 


J^ 


OUTPUT  TO 

TEMPORARY 

STORAGE 


\/ 


(RETURN^ 


YES 


JiJ^ 


ADJUST  POSITION 

VALUES  FOR 
PLANE  MOVEMENT 


NO 


^  LOOP  ^ 


Figure  5.4  (Cont)   Subroutine  HSTAR.     Home  to  Start. 
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(^^ 


^1^ 

PRINT    "START  TO  FINISH* 
AND  START/FINISH  POINTS 


J^ 


INPUT  THREE  TUPLE  OF  PTs 
ALREADY  CALCULATED 


[loop^ — ) 


.2^ 


ADJUST  MAXIMUM  STEP  SIZE 


.2^ 


ADJUST  FOR  NEGATIVE 
X  INCREMENT 


jsj^ 


ADJUST  FOR  X  PLANE 
MOVEMENT 


^^ 


CALCULATE  Y  INCREMENT 
FROMX 


JiJ^ 


ADJUST  FOR  PLANE 

MOVEMENT  IN  THE 

Y  DIRECTION 


C3 


YES 


Y  INCREMENT 
LESS  THAN  MAX 
ALLOWED  Y 
INCREMENT. 


NO 


Figure  5.5  Subroutine  STARF.    Start  to  Finish. 
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SCALE  X  INCREMENT 
BASED  ON  Y  CALCULATED 


NO 


\^ 


YES 


3/ 


X  INCREMENT  IS  0.0 
T 


CALCULAT  Y 
INCREMENT  FROM  X 


NO 


\^ 


Y  INCREMENT 
EQUALS  MAXIMUM 

Y  STEP  SIZE 


YES 


^ 


^ 


J^ 


Y  INCREMENT  IS  0.0 


CALCULATE  Z  INCREMENT 
FROM  X  AND  Y  INCREMENTS 


A^ 


SCALE  X  AND  Y  INCREMENT 
FROM  Z  INCREMENT 


^^ 


ADJUST  Z 
INCREMENT 

FOR   Z 

MOVEMENT 

ONLY 


:^ 


CALCULATE  THREE-TUPLE  OF 
POSITION  POINTS 


^ 


Figure  5.5  (Cont)  Subroutine  STARF.     Start  to  Finish. 
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CHECK  AND  ADJUST 
FOR  PLANE  MOVEMENT 


\/ 


LAST  POSITION 
UPDATED 


JiJ^ 


OUTPUT  TO 

TEMPORARY 

STORAGE 


[return^ 


-)(^LOOP    ^ 


Figure  5.5  (Cont)  Subroutine  STARF.     Start  to  Finish. 
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[  START  ^ 


\1/ 

PRINT  "FINISH  TO  START" 
AND  START/FINISH  POINTS 


J^ 


INPUT  THREE  TUPLE  OF  PTs 
ALREADY  CALCULATED 


(W) — ^ 


\i/ 


ADJUST  MAXIMUM  STEP  SIZE 


CHECK 

TOR    FINISH   EQUAI 

TO    START 

NO 


YES 


> 


LOOP 
BYPASS 


SCALE  MOVEMENT  INCREMENT 

FOR  MAXIMUM  MOVEMENT  ALLOWED 

IN  THREE  DIMENSIONS 


J^ 


ADJUST  FOR  NEGATIVE 
INCREMENTS 


J^ 


UPDATE  NEW 
POSITION 


:^ 


OUTPUT  NEW  POSITION 
ONTO  TERMINAL 


Figure  5,6  Subroutine  FSTAR.    Finish  to  Start. 
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LOOP 
BYPASS 


^ 


YES 


\/ 


LAST  POSITION 
UPDATED 


A^ 


OUTPUT  TO 

TEMPORARY 

STORAGE 


\1/ 

(^return] 


YES 


\/ 


ADJUST  POSITION 

VALUES  FOR 
PLANE  MOVEMENT 


YES 


^ 


LOOP 
BYPASS 


NO 


•^    LOOP    ^ 


Figure  5.6  (Cont)  Subroutine  FSTAR.  Finish  to  Start. 
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r  START  ^ 


\1/  

PRINT  "  FINISH  TO  HOME" 
AND  START/FINISH  POINTS 


JsJ^ 


INPUT  THREE  TUPLE  OF  PTs 
ALREADY  CALCULATED 


C'-OQPJ )  ADJUST  MAXIMUM  STEP  SIZE 


YES 


^ 


LOOP 
BYPASS 


SCALE  MOVEMENT  INCREMENT 
FOR  MAX   ALLOWED  IN  THREE  DIM 


Jj^ 


UPDATE  NEW  POSITION 


YES 


^ 


LOOP 
BYPASS 


Figure  5.7  Subroutine  FHOME.      Finish  to 
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e. 


LOOP 
BYPASS 


f 


\/ 


■) 


YES 


\/ 


LAST  POSITION 
UPDATED 


2^ 


OUTPUT  TO 

TEMPORARY 

STORAGE 


(^  RETURN^ 


YES 


Al/ 


ADJUST  POSITION 

VALUES  FOR 
PLANE  MOVEMENT 


NO 


■^  LOOP  ^ 


Figure  5.7  (Cont)  Subroutine  FHOME.     Finish  to  Honne. 
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[  START  ^ 


& 


F2 


^kl 

INPUT  THREE  TUPLE  OF  PTs 
ALREADY  CALCULATED 


YES 


^ 


XD 


CALCULATE  X 
INCREMENT 


f 


SCALE  DT 


YES 


"TK" 


YES 


^ 


ILi 


DELXrO.O 
DT=DELT 


\/ 


CALCULATE    Y 
INCREMENT 


igure  5.8  Subroutine  SFPAR  .    Parametric  Start  to  Finish. 
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F2 


SCALE  DT 


K 


YES 


YES 


J^ 


DELY=0.0 
DTrDELT 


CALCULATE    Z 
INCREMENT 


(r 


SCALE  DT 


YES 


/\ 


YES 


liJLl 


DELZ=0.0 
DT=DELT 


Figure  5.8  (Cont)  Subroutine  SFPAR.      Parametric  Start  to  Finish. 
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& 


YES 


JvJ^ 


DT  =  TF  -  T 


CALC  DELX,DELY,DELZ 
FROM  DT 


:^^ 


CALC  NEXT  POSITION 


\/ 


T  =  T  +  DT 


NO 


X5) 


YES 


OUTPUT  TO 

TEMPORARY 

STORAGE 


\1/ 

[return^ 


Figure  5.8  (Cont)  Subroutine  SFPAR.      Parametric  Start  to  Finish. 
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VI.  PATH  FOLLOWING  ROBOT  SIMULATIONS 

A.       INTRODUCTION 

Interfacing  the  path  program  developed  in  Chapter  Five  with  the  DSL 
program  selected  in  Chapter  Four  has  resulted  in  some  interesting 
observations.  Since  the  overall  objective  of  the  thesis  is  to  get  a  cartesian 
robot  arm  to  follow  a  path  that  was  precalculated  it  would  seem  that  all 
that  needs  to  be  done  is  to  generate  a  set  of  coordinates  and  sequentially 
feed  these  values  into  the  robot  arm.  However  the  DSL  program,  which  is 
chosen  to  simulate  the  robot  arm  has  certain  control  parameters  that  work 
within  the  bounds  of  DSL  and  do  not  give  an  indication  of  the  type  of 
hardware  necessary  to  accomplish  the  task.  For  example,  DSL  has  several 
options  for  the  way  it  accepts  inputted  data  and  applies  this  data  to  the 
robot  arm.  Two  of  these  were  discussed  in  Chapter  Four  but  never  explained. 
If  the  robot  were  to  receive  a  set  of  coordinates  it  is  fairly  easy  to  see  how 
to  make  the  STEP  input,  but  not  readily  apparent  on  how  to  take  the  set  of 
coordinates  and  get  a  RAMP.  Subdividing  the  coordinates  in  a  linear  fashion 
and  inputting  these  at  a  constant  rate  is  one  answer,  but  that  is  the  same  as 
defining  smaller  increments  in  the  path  data  and  is  less  accurate. 

Another  problem  occurs  when  using  position  as  a  means  of  deciding 

whether  or  not  you  have  reached  the  end  of  the  step  and  are  ready  to  input 

the  next  set  of  data.  Using  position  can  be  accurate  at  the  points  that  were 

desired  in  the  path  equation  but  you  do  not  really  have  sufficient  control 

over  the  path.  It  is  probably  better  to  set  a  designated  time  interval  for 
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each  path  length  and  use  this  to  control  the  rate  at  which  the  robot  arm  gets 
the  next  set  of  coordinate  points.  This  was  also  discussed  in  Chapter  Four. 

B.      SCALING 

The  path  generating  program  also  has  made  no  mention  of  the  length  of 
the  robot  arms.  Each  robot  arm  can  be  of  different  length  and  blindly 
assuming  that  the  robot  can  follow  all  of  the  points  without  any  problem 
may  lead  to  serious  problems.  The  DSL  program  also  assumes  that  the 
lengths  of  the  arms  are  sufficient.  Some  prescaling  may  be  needed  to  make 
all  of  the  movements  within  the  range  of  the  arm.  This  would  take  the 
maximum  value  of  the  coordinate  listing  and  scale  all  of  the  coordinate 
points  with  the  relative  length  of  the  arm  in  that  direction.  This  would  lead 
to  a  distortion  of  the  equations  used  to  generate  the  input.  An  alternative 
method  would  be  to  scale  everything  with  reference  from  the  largest  value 
to  a  unit  length,  causing  no  distortion,  and  then  setting  all  of  the  values  on 
the  smaller  robot  arms  to  a  max  value  while  that  position  is  being 
attempted.  It  would  be  similar  to  running  the  arm  up  against  a  stop  point 
and  maintaining  that  value  for  all  of  the  points  that  occur  outside  of  that 
region. 

It  is  the  latter  method  that  will  be  chosen  in  this  thesis.  After  the 

Path  generation  program  has  been  completed  a  path  boundary  program  will 

take  the  maximum  value  of  the  input  coordinate  points  and  scale  the  array 

so  that  this  value  is  one.  For  now  we  will  assume  that  all  arms  are  of  equal 

length  and  have  a  value  of  one.  Of  course  it  may  be  that  a  user  would  want  a 

larger  scale  value  and  with  a  minor  modification  that  can  be  entered. 
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C.      TEST  RUNS 

There  are  several  types  of  paths  that  need  to  be  considered  and  tested 
to  insure  proper  operation.  They  are: 

1.  Linear 

2.  Large  curvature 

3.  Small  curvature 

4.  Repeated  motions 

The  curvature  paths  can  be  observed  by  using  conic  sections,  which 
include  mainly  circles,  ellipses,  parabolas,  and  hyperbolas,  or 
transcendental  functions,  which  are  mainly  composed  of  sine,  cosine,  or 
tangential  functions.  Since  it  would  be  difficult  to  test  all  of  the 
different  types  of  curves  we  have  arbitrarily  chosen  a  circular  function 
and  a  sine  function  as  test  platforms.  For  a  repeated  motion  the  right 
circular  helix  has  been  chosen.  These  movements  will  also  insure  proper 
operation  as  a  curve  moves  tangent  to  a  coordinate  plane.  The  final  motions 
will  be  that  of  a  straight  line  in  all  of  the  coordinate  axes  and  one  parallel 
to  each  coordinate  axis.  The  goal  is  to  demonstrate  flexibility  of  the 
program  and  find  problem  areas. 

Table  6.1  summarize  the  test  runs  that  that  will  be  studied.  Each  of 
these  runs  will  be  applied  to  both  the  STEP  and  RAMP  excitations  and 
Position  and  Velocity  Methods  discussed  in  Chapter  4.  Figure  6.1 
summarizes  the  equations  used  and  the  modifications  to  the  PATH  program. 
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TABLE  6.1 

TEST  RUNS 

TEST  RUN 

EXCITATION 

METHOD 

MOTION 

1(a) 

STEP 

POSITION 

Linear  motion 

1(b) 

STEP 

VELOCITY 

1(c) 

RAMP 

POSITION 

1(d) 

RAMP 

VELOCITY 

2(a) 

STEP 

POSITION 

Circular  motion 

2(b) 

STEP 

VELOCITY 

2(c) 

RAMP 

POSITION 

2(d) 

RAMP 

VELOCITY 

3(a) 

STEP 

POSITION 

Helical  Motion 

3(b) 

STEP 

VELOCITY 

3(c) 

RAMP 

POSITION 

3(d) 

RAMP 

VELOCITY 

4(a) 

STEP 

POSITION 

Sinusoidal 

4(b) 

STEP 

VELOCITY 

4(c) 

RAMP 

POSITION 

4(d) 

RAMP 

VELOCITY 

D.       RESULTS 

1.  Linear  Motion 

Linear  motion  is  one  of  the  more  basic  movements  and  also  one  of 
tine  hardest  to  describe.  For  example  a  linear  movement  parallel  to  any 
axis  is  hard  to  put  to  an  equation  while  in  the  cartesian  coordinate  system 
if  not  using  parametric  equations.  Two  linear  motions  were  chosen  to 
show  that  the  program  works  correctly  in  both  the  cartesian  and  the 
parametric  equation  form.  They  are  described  by: 
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Linear  path  1 : 

Equation  1 

(1.0,0.25,0.25) 

to 

(0.25,1.0,1.0) 

Equation  2 

(0.5.1.0,1.0) 

to 

(1.0,0.5,1.0) 

Linear  path  2: 

Equation  1 

(1.0,0.25,0.5) 

to 

(1.0,0.75,.05) 

Equation  2 

(0.75,1. 0,.05) 

to 

(0.25,1.0,0.5) 

Equation  3 

(0.0,1.0,0.0) 

to 

(0.0,1.0,1.0) 

Equation  4 

(0.75,0.25,0.25) 

to 

(0.25,.075,0.75) 

Linear  Path  1  was  calculated  on  an  early  path  generating  program  using  only 

cartesian  coordinates  while  Linear  Path  2  was  generated  on  the  final  path 

program  mixing  cartesian  and  parametric  equations.  Linear  Path  1  was 

re-run  on  the  final  path  program  to  insure  proper  operation.  Parametric 

equations  were  used  to  generate  the  paths  parallel  to  the  axis  while 

cartesian  equations  were  used  to  generate  diagonal  paths  (however 

parametric  equations  would  be  just  as  easy  or  easier  to  use).  Equations 

used  to  describe  Linear  Path  1  and  2  are  outlined  in  Figure  6.1 .  Results  of 

the  Linear  Path  1  and  2  are  displayed  in  Figures  6.2  (a)-(d)  and  6.3  (a)-(d), 

respectively,  and  show  a  3-dimensional  view  of  the  desired  path,  the  Robot 

arm  position  with  respect  to  time  for  each  degree  of  motion,  each  of  the 

axis  plane  projections,  and  the  error  between  the  desired  path  inputted  and 

the  final  arm  position  resulting  from  the  simulations  for  the  STEP 

excitation,  Position  method  of  the  conditions  shown  in  Table  6.1  (la).  The 

other  conditions  listed  in  Table  6.1  for  linear  motion  were  run  but  showed 

nothing  significant  that  will  not  be  discussed  in  more  detail  in  the  next 

section  on  circular  motion  and  are  located  in  Appendix  H.  Observing  the  time 

versus  distance  curves  shows  smooth  motion  for  the  size  of  step  inputted 

while  the  plane  projection  plots  show  that  the  path  appears  to  be  following 
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the  equations  accurately.  Path  error  generated  in  each  of  the  coordinate 
axes  show  that  each  of  the  arm  axes  are  within  the  desired  0.001  inches  of 
accuracy  [VALMIN  in  the  simulation  program].  The  error  developed  by  the 
path,  model,  and  simulation  will  be  discussed  later  in  this  chapter. 

2.  Circular  Motion 

Test  runs  2(a) -(d)  studied  the  effects  of  circular  motion.  First  a 
single  quadrant  (R0BPATH3. FORTRAN)  was  run  to  see  the  effects  on  the 
curvature.  Then  when  attempting  to  do  a  right  circular  helix 
(R0BPATH5.F0RTRAN)  a  problem  was  noted  when  the  x  increment  was 
approaching  a  tangent  with  the  y  coordinate  axis  (at  x=0).  That  was  when 
the  decision  to  do  a  complete  circle  was  made  (R0BPATH4.FPRTRAN).  Being 
more  interesting  than  one  quadrant  most  of  the  testing  will  be  done  on  the 
complete  circular  motion. 

The  circular  path  generated  was  linear  from  home  to  start  of  the 
circular  motion  using  the  HSTAR  subroutine  with  a  maximum  step  size  of  0„1 
units.  Then  the  path  followed  the  equations: 
Equation  1 : 

y  =  ((.25  -  (x-.5)2)i/2  +  .5)  [eqn  6.1] 

z  =  0.5  [eqn  6.2] 

Equation  2: 

y  =  (-(.25  -  (x-.5)2)i/2  +  .5)  [eqn  6.3] 

z  =  0.5  [eqn  6.4] 


which  describe  circular  motion  in  the  XY  plane,  with  an  increment  maximum 
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step  size  of  0.001 .    The  change  in  maximum  step  size  was  implemented  to 
show  that  the  path  can  be  made  smooth.  This  technique  will  be  implemented 
only  for  the  position  method  techniques  because  implementing  this 
technique  for  the  velocity  method  would  require  significant  changes  in  the 
DSL  simulation  and  would  affect  the  outcome  of  the  simulation.  For  the 
velocity  method  a  maximum  step  size  of  0.01  units  was  used  for  all  of  the 
paths.  After  finishing  the  path  movement  a  linear  path  to  home  was 
completed  with  a  maximum  step  size  of  0.1  units. 

Figure  6.4  (a)  shows  time  versus  simulated  arm  position  for  each  of 
the  coordinate  axes  while  Figure  6.4  (b)  shows  the  plane  projections  for 
each  of  the  coordinate  axes.  To  do  this  motion  a  total  of  505  coordinate 
points  were  computed  and  inputted  to  the  simulation.  As  each  step  reached 
the  desired  accuracy,  stipulated  by  VALMIN  of  the  simulation  program 
[Position  Method],  the  next  three  tuple  of  position  points  were  inputted  and 
the  simulation  was  run  again.  The  curve  is  very  smooth  and  quite  accurate. 
Figure  6.4  (c)  plots  the  error  between  each  of  the  desired  inputted  points 
and  the  resulting  simulation  movement.  As  you  can  see  the  accuracy  at  the 
final  positions  are  well  within  the  accuracy  described  by  the  desired 
Position  Method  value  of  0.001  inches. 

Figures  6.5  (a)-(c)  represent  the  circle  path  using  the  RAMP 

excitation  along  with  the  Position  Method  of  simulation  control.  The  time 

response  curves  show  a  more  linear  response  especially  near  discontinuities 

where  radical  changes  in  slope  occur.  For  example  when  comparing  Figures 

6.4  (a)  and  6.5  (a),  near  the  peaks  and  valleys  of  the  time  response  curves, 

there  is  a  definite  rounding  to  the  STEP  Excitation  time  response  curves. 
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Also  notice  that  there  is  a  significant  reduction  in  time  when  using  the 

RAMP  Excitation  over  the  STEP  Excitation.  Path  error  is  nearly  the  same  for 

both  the  STEP  Excitation,  Figure  6.4  (c),  and  the  RAMP  Excitation,  Figure  6.5 

(c),  however  it  is  interesting  to  note  that  the  RAMP  Excitation  error 

decreases  as  a  function  of  time.  There  is  also  a  time  delay  at  the  start  of 

the  time  response  curves  which  is  due  to  the  first  step  of  the  RAMP  being 

zero  and  the  desired  path  being  zero  at  the  same  time.  This  phenomenon  will 

be  observed  in  some  of  the  latter  RAMP  excitation  curves  also. 

Figures  6.6  (a)-(l)  and  Figures  6.7  (a)-(k),  for  the  STEP  and  RAMP 

excitation,  respectively,  show  the  effects  of  inputting  the  "next"  position  at 

shorter  intervals  of  time.  This  method  is  called  the  Velocity  Method.  Due  to 

the  large  number  of  figures  required  only  the  XY  plane  projection  and  the 

path  errors  for  each  axis  will  be  shown.  The  test  runs  for  the  Velocity  and 

Position  methods  are  listed  in  Table  6.2.  Comparison  of  the  XY  plane 

projections  for  the  STEP  Excitation,  Velocity  Method  show  that  the  error  in 

the  paths  stay  to  within  0.001  inches  until  the  inputted  time  interval 

reaches  0.008  seconds  when  there  is  a  sharp  increase  in  the  path  error  in 

the  X  direction.  The  Y  and  Z  path  error  remain  constant  until  0.005  seconds 

when  they  start  rising  at  about  the  same  rate  as  the  X  error.  When  RAMP 

Excitation  was  applied  to  the  simulation  program  with  Velocity  Method, 

similar  results  were  experienced  except  that  the  error  curves  exceeded 

0.001  inches  with  a  0.001  sees  shorter  input  interval  in  all  three  axis. 

Table  6.2  contains  an  estimation  of  the  maximum  error  for  both  the  STEP 

and  RAMP  Excitation  and  the  results  are  plotted  in  Figures  6.8  (a)-(c). 

Figure  6.6(1)  was  used  to  verify  the  DSL  Program  save  function  to  insure 
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that  the  same  plot  was  produced  when  the  save  statement  was  larger  than 
the  velocity  step  increment.  For  example,  whether  the  save  statement  for  a 
VAIMIN  of  .001  is  saved  at  time  intervals  of  .005  or  .001 ,  we  arrive  at  the 
same  result. 

TABLE  6.2 
CIRCULAR  MOTION  TEST  RUNS/  VELOCITY  METHOD 


POSITION  METHOD: 

PATH  PROGRAM 
SIMULATION  PROGRAM 
DATA  FILE 

RGURE  NO. 

6.4  (a)-(d) 

6.5  (a)-(c) 
VELOCITY  METHOD: 

PATH  PROGRAM 
SIMULATION  PROGRAM 
DATA  FILE 


R0BPATH4.F0RTRAN 

R0BSIM2T.F0RTRAN  or  RSIM2T6. FORTRAN 

PATH4.DATA 
EXCITATION 
STEP 
RAMP 


INPUT 
INTERVAL/fFlG^ 


0.020 
0.015 
0.010 
0.008 
0.007 
0.006 
0.005 
0.004 
0.003 
0.002 
0.001 

o.oor 


a) 
b) 
c) 
d) 
e) 
f) 

g) 

h) 
(i) 

(i) 

k) 
I) 


ROBPATH4.FORTRAN 

R0BSIM2V.F0RTRAN  or  RSIM2V6.FORTRAN 
PATH4.DATA 
PEAK  ERROR  [milli-inches] 
STEP  EXCITATION  (FIG  6.6)      RAMP  EXCITATION  (FIG  6.7) 


}L 


X 


X. 


X. 


0.10 

0.10 

0.25 

1.40 

2.50 

4.90 

8.00 

15.0 

30.0 

80.0 

300.0 

300.0 


0.60 
0.60 
0.60 
0.60 
0.60 
0.50 
1.00 
2.50 
6.50 
20.0 
85.0 
85.0 


0.15 
0.18 
0.18 
0.18 
0.18 
0.25 
0.70 
1.80 
4.50 
12.0 
60.0 
60.0 


0.10 
0.10 
0.08 
0.25 
0.90 
2.50 
5.00 
10.0 
22.0 
50.0 
200.0 


0.50 
0.50 
0.50 
0.50 
0.50 
0.50 
0.50 
0.60 
0.60 
6.00 
35.0 


0.15 
0.20 
0.18 
0.18 
0.19 
0.19 
0.19 
0.30 
1.40 
5.00 
30.0 
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Differences  in  the  Position  and  Velocity  metiiod  can  be  seen  by 
comparing  STEP  Excitation,  Position  Method  with  STEP  Excitation,  Velocity 
Method,  where  the  desired  position  is  inputted  at  0.008  seconds  so  that  the 
total  time  is  around  4.0  seconds.  The  X  path  error  is  larger  in  the  Velocity 
Method  while  the  Y  and  Z  path  error  is  less.  The  plane  projections  appear 
Identical,  however  the  time  response  are  dissimilar  in  both  the  X  and  Y 
direction.  The  X  direction  is  more  rounded  through  the  circular  part  for  the 
Velocity  Method  [see  Appendix  H  for  the  Velocity  Method  Time  Response 
Figures,  VALMIN=  0.008]. 

3.  Helical  Motion 

With  the  current  path  formation  program  it  is  difficult  to  do 

repetitive  movements,  unless  all  of  the  dimensions  are  repetitive  in  nature. 

The  circle  discussed  above  was  repetitive  in  all  3  dimensions.  When  an 

attempt  is  made  to  form  even  the  simplest  right  circular  helix  it  is 

necessary  to  keep  track  of  the  number  of  revolutions  and  the  process 

becomes  more  difficult.  To  solve  this  problem  a  parametric  subroutine  was 

added  to  the  path  generating  program  that  allows  X,  Y,  and  Z  to  be  expressed 

as  a  function  of  a  single  variable,  such  as  time  or  displacement.  This 

reduces  the  number  of  equations  needed  to  do  a  right  circular  helix  to  three. 

The  parametric  equations  and  modifications  to  ROBPATH5  program  are  listed 

in  Figure  6-9.  Two  runs  were  made  for  a  three  and  six  revolution  right 

circular  helix  and  are  show  in  Figures  6.10  (a)-(d)  and  6.12  (a)-(d)  for  STEP 

Excitation  and  Position  Method,  and  Figures  6.1 1  (a)-(c)  and  6.13  (a)-(d)  for 

RAMP  Excitation,  Position  method.  Figure  6.14  and  6.15  (a)-(c)  show 
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different  STEP  Excitation,  Velocity  Method  Simulations.  To  arrive  at  the 
same  accuracy  as  the  Position  Method  it  is  necessary  to  have  Velocity 
Method  Input  Intervals  (VMM)  of  0.020  seconds  which  is  about  the  same  as 

(number  of  points)  x  (VMII)  =  Position  completion  time 

which  is  as  expected.  Figure  6.18  shows  the  VMII  versus  path  errors 
overlayed  onto  the  circular  path  Figures  6.8  (a)  and  (b). 

To  insure  proper  operation  of  the  parametric  equation  the  start  point 
for  the  independent  variable  must  be  smaller  than  the  finish  point.  In  these 
examples  the  independent  variable  t  was  chosen  to  be  in  radians  and  the 
equations  adjusted  as  needed  to  get  the  desired  output.  Figure  5-8  shows 
the  flowchart  of  the  subroutine  used.  The  three  revolution  right  circular 
helix  took  231  input  position  points  to  simulate  while  the  six  revolution 
right  circular  helix  took  420  input  positions.  Figures  6.1 7  (a)  and  (b) 
confirm  that  the  VMII  for  the  six  revolution  right  circular  helix  with  ramp 
excitation  would  be  between  0.020  and  0.015  seconds.  Calculating  directly 
indicates  that  a  value  of  0.01 55  would  be  adequate  to  drive  the  Velocity 
Method  path  error  under  0.001   inches. 

4.  Sinusoidal  Motion 

Two  sinusoidal  equations  that  were  generated  are: 

Y  =  f(X)     =  ABS  (  SIN  (  X  ) )  [eqn  6.5] 

Z  =  f(X,Y)  =  ABS  (COS  (2*PrX))  [eqn  6.6] 

The  STEP  excitation,  Position  method  results  are  shown  in  Figure  6.19 
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(a)-(d)  along  with  a  three  dimensional  diagram  of  the  motion,  while  the 
RAMP  Excitation,  Position  Method  results  are  shown  in  Figure  6.20  (a)-(c). 
The  plots  are  correct  but  are  not  of  much  use.  There  are  a  couple  of  points 
of  discontinuity  which  are  caused  by  interaction  between  the  large 
distances  between  steps  inputted  and  the  absolute  function. 

E       POSITION  ACCURACY 

When  comparing  STEP  or  RAMP  Excitations,  with  Position  Method 
simulation  and  varying  the  value  of  VALMIN,  which  is  the  error  difference 
between  the  desired  position  (RX,RY,  RZ)  and  the  motor  position  (C1,  C2,  C3), 
while  the  Path  Error  is  the  difference  between  the  desired  position 
(RX,RY,RZ)  and  the  simulated  position  (CX,  CY,  CZ).  There  is  a  limit  to  how 
accurate  the  simulation  can  get  by  just  changing  VALMIN.  Table  6.3  list  the 
error  for  each  of  the  axis  when  following  the  circular  path  and  the  three 
revolution  right  circular  helix  using  the  Position  Method.  Only  50  position 
steps  were  utilized  to  keep  computing  time  reasonable. 

Since  there  was  no  movement  in  the  Z  direction  for  the  circular  path 
there  is  no  Path  Error  in  that  direction.  This  was  the  reason  that  the  helical 
path  was  run  also.  In  most  cases  the  path  error  is  half  of  the  desired 
VALMIN.  The  only  disagreement  is  in  the  Y  direction  where  there  seems  to 
be  a  limit  where  the  Y  path  error  levels  off  regardless  of  the  value  of 
VALMIN.  This  is  around  0.080  milli-inches  for  the  Circular  Path  and  0.060 
milli-inches  for  the  Helical  Path.  Smaller  values  for  the  circular  path  and 
the  helical  path  indicate  a  lower  limit  in  the  simulation  program  to  arrive 
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at  a  desired  accuracy.  XY  Plane  Projection  and  X,  Y,  and  Z  Path  Errors  are  for 
the  Velocity  Method  are  located  in  Appendix  H. 

TABLE  6.3 
ACCURACY  USING  POSITION  METHOD 


VALMIN 

CIRCULAR  PATH  ERROR  [milli-in] 

HELIX  PATH  ERROR 

[milli-in] 

[mllij-in] 

2( 

Y 

z 

Fia 

X 

Y 

z 

FIG, 

50.0 

— 

— 

-— 

— 

25.0 

25.0 

1.5 

H-4(a) 

10.0 

5.0 

5.0 

0.0 

H-3(a) 

5.0 

5.0 

0.1 

H-4(b) 

5.0 

2.5 

2.5 

0.0 

H-3(b) 

2.5 

2.5 

0.12 

H-4(c) 

1.0 

0.5 

0.5 

0.0 

H-3(c) 

0.5 

0.5 

0.16 

H-4(d) 

0.5 

0.25 

.25 

0.0 

H-3(d) 

0.25 

0.3 

0.11 

H-4(e) 

0.1 

0.05 

0.08 

0.0 

H-3(e) 

0.05 

0.06 

0.05 

H-4(f) 

0.05 

0.025 

0.08 

0.0 

H-3(f) 

0.03 

0.06 

0.03 

H-4{g) 

0.01 

0.018 

0.09 

0.0 

H-3(g) 

FAILED 

0.005 

0.02 

0.08 

0.0 

H-3(h) 

0.001 



FAILED    

Failed  runs  were  due  to  large  computation  times,  or  problems  with  the 
simulation  in  resolving  integrations.  Several  different  integration 
techniques  were  attempted,  along  with  various  DELT  and  DELS  values,  but 
the  runs  did  not  improve  or  complete  more  than  a  few  path  points  out  of  the 
fifty  in  the  path,  while  the  paths  that  were  completed  successfully  were 
rapidly  done. 


F.    ffiROR 

There  are  several  areas  of  error  involved  when  trying  to  get  accurate 
movement  of  the  robot  arm.  They  include: 
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1 .  Motor  simulation  model  error  in  assuming  that  the  model  can  be 
reduced  to  a  second  order  system. 

2.  Robot  Kinematics  and  Dynamic  equation  assumptions. 

3.  Computer  roundoff  errors.  While  DSL  runs  in  double  precision  there  are 
still  minimum  accuracies  that  are  fairly  large  when  compared  to  the 
accuracies  that  are  needed  in  the  simulation.  All  programs  other  than 
DSL  are  single  precision  and  susceptible  to  large  truncation  and 
round-off  errors. 

4.  Program  design  errors.  The  largest  error  involved  in  the  program  is  in 
assuming  a  minimum  value  for  several  of  the  operations.  In  the 
Position  Method  the  value  VALMIN  (E)  is  chosen  to  be  a  given  value.  The 
difference  between  C1  and  CX,  C2  and  CY,  and  C3  and  CZ  is  another 
error.  Ideally  these  values  will  be  the  same,  but  that  is  not  always  the 
case.  In  the  previous  section  the  fact  that  the  error  in  the  Y  direction 

can  be  larger  than  the  value  of  VALMIN  shows  that  CY  is  not  the  same 
as  C2  by  a  considerable  amount.  This  shows  a  discrepancy  in  the 
computer  simulation  model  and  in  the  way  that  Path  Error  was  defined. 
The  effect  of  these  errors  is  to  create  a  sphere  of  error  around  each  of 
the  position  points.  Not  necessarily  the  same  in  all  three  coordinate  axes,  it 
is  easier  to  visualize  a  sphere  following  the  desired  path.  As  the  Robot  arm 
follows  the  desired  path  around,  the  error  effectively  becomes  a  cylinder 
around  the  desired  path.  Added  to  the  non-linearity  of  the  Robot  Arm 
between  points  the  error  cylinder  can  become  quite  large. 
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Path  Equations  for  Linear  Path  1 
EQUAT1         =         0.500 
EQATIY         =         1.25 -X 

EQUAT2        =         1.00 
EQAT2Y        =         1.50 -X 

Start  and  Finish  Points  For  Linear  Path  2 
XS1     =     1.00  XS2    =    0.50 

XF1     =     0.25  XF2     =     1.00 


Path  Equations  for  Linear  Path  2 
PEQX  1      =      0.75 
PEQY  1      =      0.5  +  0.25  T 
PEQZ  1      =      0.00 


PEQX  2 

= 

0.75  -0.5T 

PEQY  2 

= 

0.75 

PEQZ  2 

= 

0.25 

PEQX  3 

= 

0.25 

PEQY  3 

= 

0.75 -0.5  T 

PEQZ  3 

= 

0.00 

PEQX  4 

" 

0.25 

PEQY  4 

= 

0.25  +  0.75  T 

PEQZ4 

= 

1.00 

EQUAT5 

= 

X 

EQAT5Y 

~ 

1.5 -X 

Start  and  Finish  Points  For  Linear  Path  2 
TS   =   0.00  XS5   =   0.50 

TF   =    1.00  XF5   =    1.00 


Figure  6.1     Path  Equations  and  Program  Modifications 

for  Linear  Path  1  and  2. 
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Figure  6.2(c)    Linear  Path  1,  STEP  Excitation,  Position  Method. 

X,  Y,  and  Z  Path  Error. 
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Figure  6.2(cl)    3-D1mensional  View  of  Linear  Path  1 
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Figure  6.3(c)    Linear  Path  2,  STEP  Excitation,  Position  Method. 

X,  Y,  and  Z  Path  Error. 
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Figure  6.3(d)    3-Dimensional  View  of  Linear  Path  2. 
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Figure  6.4(c)    Circular  Path,  STEP  Excitation,  Position  Method. 

X,  Y,  and  2  Path  Error. 
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Figure  6.4(d)    3-D1mens1ona1  View  of  Circular  Path. 
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Figure  6.5(c)    Circular  Path,  RAMP  Excitation,  Position  Method. 

X.  Y.  and  Z  Path  Error. 
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Figure  6.8  (a)    Velocity  Method  STEP  Excitation  Error. 
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Figure  6.8  (b)    Velocity  Method  RAMP  Excitation  Error. 
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Parametric  Equations 
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Figure  6.9    Parametric  Equations  and  Path  For  Right  Circular 

Helix's  1  and  2. 
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X,  Y,  and  Z  Path  Error. 
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Figure  6  10(d)    3-D1mensional  View  of  Helix  Path  1 
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Figure  6.12(c)    Helix  Path  2,  STEP  Excitation,  Position  Method. 

X,  Y,  and  Z  Path  Error. 
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Figure  6.12(d)    3-Dimensiona]  View  of  Helix  Path  2. 
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X.  Y.  and  Z  Path  Error. 
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Figure  6. 1 8  Right  Circular  Helix  Error  Comparison. 
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Figure  6.19(c)    Sinusoidal  Path,  STEP  Excitation,  Position  Method. 

X,  Y,  and  Z  Path  Error 
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Figure  6.19(d)    3-Dimensional  View  of  Sinusoidal  Path. 
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Figure  6.20(c)    Sinusoidal  Path,  RAMP  Excitation,  Position  Method. 

X,  Y,  and  Z  Path  Error. 
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VII.  ARTICULATED  ROBOT 

A.  INTRODUCTION 

As  mentioned  in  Chapter  Two  the  second  robot  arm  model  that  is  being 
considered  is  the  articulated  (or  revolute  )  robot  model.  Using  procedures 
similar  to  that  of  the  Cartesian  Robot  Arm  model  the  lagrangian  equations 
were  solved,  the  computer  simulation  model  was  generated,  and  the  DSL 
program  was  designed  (Kalogiros,  1987).  Appendix  G  shows  the  entire  DSL 
program  generation  along  with  single  runs  that  were  used  to  verify  and  test 
the  DSL  program  and  equations.    The  purpose  of  this  chapter  is  to  tie  the 
path  generating  program,  written  in  cartesian  coordinates,  to  the  DSL 
program  inputs,  which  are  in  articulated  coordinates.  To  do  the  coordinate 
axis  reposition  an  intermediate  transformation  to  spherical  coordinates  was 
required.  This  also  allows  spherically  generated  path  data  to  be  converted 
directly  into  articulated  coordinate  path  data.  To  interpret  the  results  it  is 
necessary  to  reconvert  the  articulated  path  back  into  spherical  or  cartesian 
coordinates  to  be  plotted. 

B.  COORDINATE  TRANSFORMATION'S 

To  transform  the  set  of  three  tuples  of  cartesian  coordinate  points  to 

the  three  tuple  of  spherical  coordinate  points  is  fairly  simple  with  just  a 

few  points  to  keep  in  mind.  Figure  7-1  (a)  shows  the  range  of  motion  for  a 

Cartesian  Robot  that  was  used  in  Chapter  Five  while  Figure  7-1  (b)  shows 

the  range  of  motion  for  the  articulated  robot.  Each  of  the  figures  show  the 
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designed  home  (reference)  position  used  and  Figure  7-2  shows  the  regions 
superimposed  upon  each  other. 

The  equations  used  to  transform  the  data  points  from  cartesian  to 
spherical  coordinates  are: 

p  =  ((XP-.5)2  +(YP-.5)2+(ZP-.5)2)i/2  [eqn  7-1] 

e  =  arccos  [((b)2+(c)2-(a)2)/(2bc)]  [eqn  7-2] 

cp  =  arccos  [((d)2+(p)2.(f)2)/(2d  p  )]  [eqn  7-3] 

where 

p  is  the  radial  unit  length  from  (.5,.5,.5)  to  the  point  being 
transformed  (XP.YP.ZP). 

8  is  the  angle  parallel  to  the  XY  plane  referenced  in  the  x 
direction  to  (XP,YP,ZP). 

cp  is  the  angle  of  p,  referenced  in  the  z  direction  through  the 
point  (.5,.5,.5). 

a,b,c,d,  and  f  are  lengths  described  in  Figure  7.1  (b). 

Equation  7-1  is  the  three  dimensional  Pythagorean  principle,  and  equations 

7-2  and  7-3  relate  the  coordinates  by  use  of  the  Law  of  Cosines  [Appendix 

A]. 

Once  in  spherical  coordinates  the  three  tuple  must  be  transformed  into 

something  useful  to  the  robot  arm.  Called  the  articulated  coordinate 

system,  it  consists  of  a  three  tuple  of  angles  referenced  to  a  spherical 

coordinate  p  maximum  of  .5  units.  Figure  7.3  shows  the  spherical  to 
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articulated  reference  system  whose  resulting  equations  are: 


CP1 

= 

e 

CP2 

= 

n  -  cp  +  arccos  (2  p  ) 

CP3 

=s 

n  -  2  (arccos[2  p  ]) 

[eqn  7-4] 
[eqn  7-5] 
[eqn  7-6] 


Several  assumptions  were  made  in  the  calculations: 

1 .  Both  of  the  articulated  robot  arms  are  of  the  same  length.  If  they  are 
not  then  the  program  will  have  to  be  adjusted  by  using  the  equations  of 
Figure  7.3. 

2.  The  robot  operates  in  the  'up'  elbow  mode.  CP1  and  CP3  are  the  same  in 
either  mode.  CP2  has  a  change  in  the  sign  of  the  arccos  term  [+  for  'up' 
elbow  and  -  for  'down'  elbow].  Figure  7.3  also  demonstrates  the  'down' 
elbow  configuration,  however  all  of  the  programs  are  solely  'up'  elbow. 
'Up'  and  'down'  elbow  considerations  are  important  when  considering 
loading  and  obstacle  avoidance. 

3.  To  make  the  programming  simple  it  was  assumed  that  the  spherical 

coordinate  system  exists  within  a  1  unit  diameter  sphere  giving  a 

maximum  radius  of  .5  units  and  arm  lengths  of  .25  units.  The 

articulated  robot  coordinate  system  really  doesn't  consider  actual  arm 

lengths  in  the  movements,  however  calculations  from  spherical  to 

articulated  must  have  magnitudes  less  than  or  equal  to  1  for  proper 

operation  of  the  cosine  and  sine  functions.  Also  any  scaling  or 

limiting  done  to  the  cartesian  and  spherical  coordinate  systems  prior 
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to  converting  to  the  articulated  coordinate  systenn  must  be  undone  or 
at  least  considered  when  interpreting  the  results. 

To  interpret  the  motion  that  the  robot  is  making  it  is  necessary  to 
convert  the  articulated  coordinate  system  back  into  either  the  spherical  or 
cartesian  coordinate  system.  Figure  7.4  contains  a  listing  of  the  available 
coordinate  conversion  routines  as  well  as  a  listing  of  the  order  in  which  to 
program  them.  The  program  TRANSFOR. FORTRAN  when  set  up  correctly 
allows  transformation  from  spherical  to  articulated  and  then  back  to 
cartesian  and  articulated  to  spherical.  The  reason  for  transforming  the 
first  series  back  to  spherical  is  to  check  that  the  final  product  is  the  same 
as  the  original  if  no  scaling  or  limiting  is  done  and  it  also  gives  the  desired 
simulation  results  when  scaling  or  limiting  is  used  prior  to  the  simulation. 

C.  SCALING  AND  LIMITING  ROUTINES 

To  insure  that  the  motion  of  the  robot  is  within  the  range  of  possible 

movement  of  the  robot  arm  it  is  necessary  to  scale  or  limit  the  motion. 

Scaling  takes  the  largest  value  of  distance  and  normalizes  the  path  three 

tuples  to  one.  There  is  also  a  scaling  factor  [SF]  which  then  scales  the 

result  to  the  desired  values.  Scaling  is  available  for  the  X,  Y,  and  Z  values 

of  the  cartesian  coordinate  system  or  for  p  in  the  spherical  coordinate 

system.  It  is  important  to  remember  that  if  the  SF  is  between  0  and  1  then 

the  path  shrinks  and  the  maximum  value  of  the  desired  path  will  be  less 

than  or  equal  to  one  while  if  the  SF  is  greater  than  one  then  the  path  will  be 

greater  than  1 .  To  insure  proper  operation  when  converting  to  the 
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articulated  system  it  is  mandatory  that  the  spherical  system  be  scaled  (if 
desired)  and  limited. 

Limiting  takes  the  results  of  the  inputted  path  data,  finds  all  the  values 
greater  than  1  in  the  desired  cartesian  path  [.5  for  p  in  the  spherical]  and 
replaces  these  values  with  1 .  This  prevents  running  the  robot  arms  past 
their  capabilities  and  with  scaling  can  lead  to  some  interesting  paths.  Both 
scaling  and  limiting  routines  are  listed  in  Figure  7.4  and  Appendix  G. 

D.    TEST  RUN  RESULTS 

To  test  the  program  for  proper  operation  of  the  DSL  program  the  paths 

generated  for  the  cartesian  robot  were  converted  and  used  as  outlined  in 

Figure  7.5.  An  adjustment  was  made  to  the  DSL  program  which  inserted  the 

first  three  tuple  as  the  'home'  position  so  that  the  paths  would  not  have  to 

be  reconstructed  using  the  spherical  home  as  the  starting  point.  This 

reduces  the  errors  in  the  first  step  to  within  an  acceptable  amount  in  both 

time  and  distance  and  does  not  have  any  effect  if  you  start  from  'home'  [the 

first  position  should  be  home  in  any  case].  Table  7.1  show  the  paths  that 

were  used  and  the  Figures  associated.  When  viewing  the  three  dimensional 

plots  there  is  a  line  from  the  last  point  to  zero  that  is  not  resident  in  the 

cartesian  data,  either  after  conversion,  bypassing  simulation  (SCPATH  . 

DATA) ,  or  after  simulation  and  conversion  back  into  cartesian  coordinates 

(DCPATH  .  DATA),  that  is  unaccounted  for  and  is  an  error  in  the  DISSPLA 

System  program.  All  of  the  runs  are  in  the  STEP  Excitation,  Position  Method 

format.  Programs  for  the  RAMP  Excitation,  Position  Method  and  the  STEP 

and  RAMP  Excitations,  Velocity  Method  were  written  and  modifications  are 
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located  in  Appendix  D,  but  no  runs  were  done  to  check  for  proper  operation 
because  of  the  time  involved  in  even  the  shortest  run.  Twenty  to  thirty  CPU 
minutes  are  not  uncommon  when  calculating  a  500  point  path  with  the 
current  simulation  model. 

TABLE  7.1 
ARTICULATED  ROBOT  ARM  PATH  MOVEMENT 


NUMBER  OF 

PATH  PROGRAM 

TYPE  PATH 

FIGURES 

POINTS 

PATH2.data 

LINEAR  1 

7.5  (a)-(e) 

80 

PATH3.data 

LINEAR  2 

7.6  (a)-(e) 

139 

PATH4.data 

CIRCULAR 

7.7  (a)-(e) 

505 

PATHS.data 

HELICAL  1 

7.8  (a)-(e) 

231 

PATHSA.data 

HELICAL  2 

7.9  (a)-(e) 

420 

PATHS.data 

LINEAR 

7.10  (aHe) 

233 

PATH7.data 

SINUSOIDAL 

7.11  (aHe) 

345 

(a)  TIME  RESPONSE  (radians) 

(b)  PATH  ERROR  (milli-rads) 

(c)  ORIGINAL  PATH  IN  3-DIMENSIONS 

(d)  DIRECT  CONVERSION  PATH  IN  3-DIMENSIONS 

(e)  SIMULATED  CONVERSION  PATH  IN  3-DIMENSIONS 

As  with  the  Cartesian  Simulation  model  for  Position  Method  the  accuracy 

was  determined  by  E  [VALMIN]  dropping  below  the  value  of  0.001  inches. 

Path  Error  is  calculated  by  taking  the  difference  between  the  desired 

position  and  the  computer  simulation  model  output  and  E  is  calculated  by 

taking  the  difference  between  the  desired  position  and  the  motor 

simulation  model  output. 

During  the  testing  of  the  Articulated  Model  some  interesting  problems 

were  encountered.  Due  to  using  paths  designed  in  cartesian  coordinates,  the 
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articulated  coordinate  inputs  to  the  simulation  are  not  controlled  as  to  the 

size  of  radian  step  from  one  point  to  the  next.  When  far  from  the  center  of 

the  Articulated  Coordinate  System  there  is  not  any  problem  as  the  radian 

step  is  small  but  as  you  get  closer  to  the  center  of  the  coordinate  system  a 

small  step  in  Cartesian  Coordinates  result  in  a  large  step  in  Articulated 

Coordinates.  Consider,  for  example,  a  step  from  one  side  of  the  Articulated 

Coordinate  System  origin  directly  through  the  center  (0.5,  0.5,  0.5)  to  the 

other  side  of  the  origin  is  translated  into  a  180  degree  or  n  radian  step  in 

the  Articulated  Coordinate  System.  Associated  with  this  problem  is  that 

with  the  simulation  we  can  assume  an  infinitely  thin,  essentially 

nonexistent  arm,  where  in  reality  there  is  a  lot  of  space  where  the  arm  can 

not  go  due  to  support  structures.  With  this  in  mind  it  makes  no  sense  to 

worry  about  going  through  the  Articulated  origin  because  the  space  is 

already  occupied.  "Home"  has  also  been  described  as  (0.5,  0.5,  0.5)  and  any 

time  that  it  is  hit  from  whatever  direction  the  Arm  whips  around  to  "home" 

in  the  articulated  coordinate  system  (0.0,  3n/2,0.0)  and  then  back  to  the 

next  position.  By  devising  a  scheme  to  prevent  the  arm  from  entering  a 

sphere  around  the  articulated  origin  most  of  the  problems  can  be  avoided. 

This  was  not  implemented  as  none  of  the  paths  went  close  to  the  origin. 

The  next  problem  came  about  in  the  definition  of  CP1 ,  which  is  the  first 

axis  in  the  Articulated  Coordinate  System.  Measured  in  radians  the  arm 

travels  from  0.0  to  2n  radians  without  any  difficult.  When  going  counter 

clockwise  from  a  number  near  2n  to  a  number  just  above  zero  a  problem 

occurs.  With  the  type  of  control  we  are  using  the  Arm  would  try  to  respond 

by  going  clockwise  back  around  the  axis  center  until  it  arrives  at  the  new 
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number.  This  is  undesirable  and  occurs  in  most  of  the  paths  that  were 
generated.  To  solve  the  problem  a  comparison  of  the  next  position  with  the 
old  position  is  done.  If  it  meets  the  criteria  described  above  then  the  old 
position  is  subtracted  from  2n  and  renamed  the  old  position.  This  is  easy 
to  implement  in  software  but  in  hardware  may  be  difficult. 

All  of  the  Path  errors  are  within  the  desired  VALMIN.  When  looking  at 
some  of  the  Path  Error  versus  time  remember  that  2n  radians  and  zero  are 
the  same  accounting  for  some  of  the  seemingly  large  errors.  This  problem 
could  have  been  remedied  by  taking  the  2n  correction  mentioned  earlier  and 
applied  it  to  both  the  motor  output  (C1)  and  the  computer  simulation  output 
(CX)  but  was  actually  only  applied  to  the  motor  output  (C1).  The  overall 
results  are  accurate  and  within  the  desired  specifications. 
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FIGURE  7-1  (a) 
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FIGURE  7.1  (b): 


Figure  7.1    (a):  Cartesian  Range  of  Motion 
(b) :  Spherical/  Articulated  Range  of  Motion 
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Cartesian  to  Spherical  Coordinate  Transformation 

p  =  ((XP-.5)2  +  (VP-.5)2  +  (ZP-.5)^  )^  ^^ 
e  =  arccos  [((b)^*  (c)^-  (a)^)/  (2bc)l 
V  =   arccos  [((d)2+(p)2-(f)2)/(2dp)] 


Choose 


((XP-  1)2  +  (YP-.5)2)i^2 
((XP-.5)2  +(YP-.5)2)^^2 


.5 
.5 


((XP  -  .5)2  +  (YP  -  .5)2  +  (ZP  -  1)2)1^2 


Where    (  XP,  YP,  ZP  )  is  the  point  being  converted  from 
Cartesian  to  spherical. 


Figure  7.2     Cartesian  to  Spherical  Coordinate  Conversion. 
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(1  ..5,.5) 


(.5,.5J) 


(.5,.5,.5) 


d1 


%\ 

ifllf        CP3       /^ 

d3 

1 
CP2J 

P            f 

Given         (p,e,(p}    as  Spherical  Coordinates 

to  find    (CP1,  CP2,  CP3)  in  Articulated  Coordinates. 

CP1      =   e 

CP2     =    180-9  +  a3  (elbow  "up") 

CP3     =   arccos  [((d2)2  +  (d3)2  -  (p)^)  /  (2  (d2)(d3))  ] 


Since  d2  =  d3  =  .25  or  max  reach  of   p=  .5 
then    a2  =  a3    and 
al    =       180  -     2(a2)     =   CP3 

=    arccos    [( .5  -  (p)^  )] 


Figure  7.3      Spherical  to  Articulated  Coordinate  Transformation. 
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PROGRAM  (FORTRAN) 

PURPOSE 

R0BPATH2 

GENERATES  LINEAR  PATH  1 

R0BPATH3 

GENERATES  LINEAR  PATH  2 

R0BPATH4 

GENERATES  CIRCULAR  PATH 

R0BPATH5 

GENERATES  RIGHT  CIRCULAR  HELIX  1 

R0BPATH5A 

GENERATES  RIGHT  CIRCULAR  HELIX  2 

R0BPATH6 

GENERATES  LINEAR  PATH 

R0BPATH7 

GENERATES  SINUSOIDAL  PATH 

TRANSFOR 

CARSP 

CARTESIAN  TO  SPHERICAL 

CARLI 

CARTESIAN  LIMITING 

CARSC 

CARTESIAN  SCALING 

SPHLI 

SPHERICAL  LIMITING 

SPHSC 

SPHERICAL  SCALING 

SPART 

SPHERICAL  TO  ARTICULATED 

ARTSP 

ARTICULATED  TO  SPHERICAL 

SPCAR 

SPERICALTO  CARTESIAN 

RSIMREV 

ARTICULATED  ROBOT  MODEL 

SINGLE  STEP  INPUT 

RSIMREV1 

ARTICULATED  ROBOT  MODEL 

MULTIPLE  STEP  INPUTS 

ROBCOMPl 

3-DIMENSIONAL  PLOTTING  ROUTINE 

Figure  7.4    Articulated  Robot  path,  transformation,  scaling,  and 

limiting  programs  and  subroutines 
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DEFINE  AND  RUN 
CARTESIAN  PATH 


PERFORM  CARTESIAN 
SCALING  OR  LIMITING 


:s^ 


TRANSFORM  CARTESIAN 
TO  SPHERICAL  COORDINATES 


:^ 


PERFORM  SPHERICAL 
SCALING  OR  LIMITING 


TRANSFORM  SPHERICAL 
TO  ARTICULATED  COORDINATES 


\/ 


PERFORM  ARTICULATED 
SIMULATION  ROUTINE 


J^ 


TRANSFORM  ARTICULATED 
TO  SPHERICAL  COORDINATES 


\^ 


TRANSFORM  ARTICULATED 
TO  SPHERICAL  COORDINATES 


j:^ 


TRANSFORM  SPHERICAL 
TO  CARTESIAN  COORDINATES 


jsi/ 


TRANSFORM  SPHERICAL 
TO  CARTESIAN  COORDINATES 


\1/  \l/ 


PLOT  BOTH  IN 

3- DIMENSIONS 

AND  COMPARE 


Figure  7.5     Articulated  Robot  Path  Outline. 
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Figure  7.6(a)    Articulated  Robot  Linear  Path  1  Time  Response. 
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Figure  7.6(b)    Articulated  Robot  Linear  Path  1  Path  Error. 
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3D  PATH  TRRJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.6(c)    Articulated  Robot  Linear  Path  1  in  3-Dimensions. 

IPATH2.DATA].  Pre-simulation. 
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3D  PRTH  TRRJECTORY 

ROBOT  PRTH  MOTIONS 


Figure  7.6(cl)    Articulated  Robot  Linear  Path  1  Direct  Conversion  Path  in 
3-Dimensions.  ISCPATH2.DATA].  No  Simulation. 


88 


. 


3D  PRTH  TRRJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.6(e)    Articulated  Robot  Linear  Path  1  Simulated  Conversion  Path  in 
3-Dimensions.  [DCPATH2.DATA].  Post  Simulation. 
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Figure  7.7(a)    Articulated  Robot  Linear  Path  2  Time  Response. 
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Figure  7.7(b)    Articulated  Robot  Linear  Path  2  Path  Error. 


3D  PATH  TRAJECTORY 

ROBOT   PRTH   MOTIONS 


Figure  7.7(c)    Articulated  Robot  Linear  Path  2  in  3-Dinnensions. 

[PATH3.DATA].  Pre-simulation. 
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3D  PATH  TRRJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.7(d)    Articulated  Robot  Linear  Path  2  Direct  Conversion  Path  in 
3-Dimensions.  ISCPATH3.DATA].  No  Simulation. 
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3D  PRTH  TRRJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7  7(e)    Articulated  Robot  Linear  Path  2  Simulated  Conversion  Path  in 
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Figure  7.8(a)    Articulated  Robot  Circular  Path  Time  Response. 
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Figure  7.8(b)    Articulated  Robot  Circular  Path  Error. 
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3D  PATH  TRAJECTORY 

ROBOT  PRTH  MOTIONS 


1 
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Figure  7.8(c)    Articulated  Robot  Circular  Path  in  3-Dimension3. 
[PATH4.DATA].  Pre-simulation. 
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3D  PATH  TRfiJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.8(d)    Articulated  Robot  Circular  Path  Direct  Conversion  Path  in 
3-Dimensions.  ISCPATH4.DATA].  No  Simulation. 
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3D  PATH  TRAJECTORY 
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Figure  7.8(e)    Articulated  Robot  Circular  Path  Simulated  Conversion  Path  in 
3-Dimensions.  [DCPATH4.DATA].  Post  Simulation. 
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Figure  7.9(a)    Articulated  Robot  Helix  Path  1  Time  Response. 
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Figure  7.9(b)    Articulated  Robot  Helix  Path  1  Path  Error. 
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Figure  7.9(c)    Articulated  Robot  Helix  Path  1  in  3-Dimensions. 
IPATH5.DATA].  Pre-simulation. 
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3D  PRTH  TRRJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.9(d)    Articulated  Robot  Helix  Path  1  Direct  Conversion  Path  in 
3-Dinnensions.  [SCPATH5.DATA].  No  Simulation. 
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3D  PRTH  TRRJECTORY 

ROBOT  PATH  MOTIONS 


Figure  7.9(e)    Articulated  Robot  Helix  Path  1  Simulated  Conversion  Path  in 
5-Dimensions.  IDCPATH5.DATA].  Post  Simulation. 
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Figure  7.10(a)    Articulated  Robot  Helix  Path  2  Time  Response. 
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Figure  7.10(b)    Articulated  Robot  Helix  Path  2  Path  Error. 
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3D  PRTH  TRRJECTORY 

ROBOT   PRTH  MOTIONS 


Figure  7.10(c)    Articulated  Robot  Helix  Path  2  in  3-Dimensions. 
[PATH5A.DATA].  Pre-simulation. 
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3D  PATH  TRAJECTORY 

ROBOT  PATH  MOTIONS 


Figure    7.10(d)    Articulated  Robot  Helix  Path  2  Direct  Conversion  Path  in 
3-Dimensions.  [SCPATH5A.DAT A].  No  Simulation. 
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Figure  7.10(e)    Articulated  Robot  Helix  Path  2  Simulated  Conversion  Path  in 
3-Dimensions.  IDCPATH5A.DATA].  Post  Simulation. 
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Figure  7. 1 1  (c)    Articulated  Robot  Linear  Path  in  3-Dimensions. 
[PATH6.DATA].  Pre-sinnulation. 
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Figure  7. 1 1  (d)    Articulated  Robot  Linear  Path  Direct  Conversion  Path  in 
3-D1mensions.  ISCPATH6.DATA1.  No  Simulation. 
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Figure  7.1 1(e)    Artlculatec!  Robot  Linear  Path  Simulated  Conversion  Path  in 
3-Dimens1ons.  [DCPATH6.DATA].  Post  Simulation. 
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Figure  7.12(c)    Articulated  Robot  Sinusoidal  Path  in  3-Dimensions. 
(PATH7.DATA].  Pre-simulation. 
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Figure  7.12(cl)    Articulated  Robot  Sinusoidal  Path  Direct  Conversion  Path  in 
3-Dimensions.  ISCPATH7.DATA].  No  Simulation. 
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Figure  7.12(e)    Articulated  Robot  Sinusoidal  Path  Simulated  Conversion 
Path  in  3-DinDensions.  IDCPATH7.DATA].  Post  Simulation. 
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VIII.    CONCLUSIONS  AND  AREAS  FOR  FURTHER  RESEARCH 

In  this  thesis  we  have  shown  that  it  is  possible  to  have  a  Robot  follow  a 
desired  path  accurately.  Seven  paths  were  used  on  both  the  Cartesian  Robot 
and  the  Articulated  Robot  with  good  results,  giving  accuracies  on  the  order 
of  10-4. 

Each  of  the  paths  were  described  using  combinations  of  cartesian  and 

parametric  equations  and  were  linked  together  by  linear  subroutines.  The 

output  of  the  path  equations  is  a  table  of  cartesian  coordinates  which  are 

designed  so  that  the  maximum  movement  in  any  direction  is  controlled  by 

the  user  (MAXOD).  Also  under  the  users  control  is  VALMIN,  which  is  the 

error  between  the  computed  position  and  the  desired  position;  the  accuracy 

(ACC),  which  allows  higher  accuracy  of  movement  in  the  Position  Method; 

the  maximum  velocity  in  each  of  the  coordinate  axes  directions  (MAXVX, 

MAXW,  and  MAXVZ);  and  the  change  in  time  increment  for  the  parametric 

routine.  The  user  also  decides  on  whether  to  plan  on  having  STEP  or  RAMP 

Excitation  to  the  Computer  Simulation  Model  and  the  method  for  which  the 

next  set  of  points  are  generated.  The  Position  Method  is  based  on  the 

accuracy  of  E  =  R  -  C  (Figure  3.2)  being  less  than  the  value  of  VALMIN. 

When  all  of  the  d.o.f.  meet  that  condition  the  the  next  position  is  inputted. 

The  Velocity  Method  ignores  accuracy  and  inputs  the  next  position  at  a  set 

time  interval.  The  advantage  of  the  Velocity  Method  is  that  you  can  get  the 

Robot  to  move  faster  with  only  a  slight  loss  in  position  accuracy  and  since 

one  of  the  leading  assumptions  made  was  that  the  maximum  error  in  any 
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step  is  less  than  the  distance  travelled  during  the  step  then  the  Velocity 
Method  is  a  good  choice  for  most  applications. 

The  Computer  Model  assumes  that  a  saturated  condition  may  be  reached 
during  the  generation  of  the  voltage  that  is  driving  ideal  joint  servo  but  the 
nonlinearities  introduced  do  not  significantly  affect  the  accuracy  of  the 
desired  movement  and  the  total  of  the  errors  stay  within  the  limit  of  the 
step  size. 

The  programs  are  based  on  the  movement  space  being  a  1  inch  cube  for 
the  Cartesian  Robot,  and  a  1  inch  diameter  sphere  for  the  Articulated  Robot. 
With  maximum  errors  of  10-4  inches  there  may  not  be  enough  accuracy  for 
some  applications.  For  example  applying  the  path  to  a  10  meter  cubed 
Cartesian  Robot  would  give  accuracies  on  the  order  of  1  mm.  For  several 
applications  this  would  not  be  sufficient  accuracy.  To  correct  this  problem 
it  might  be  a  better  approach  to  set  up  the  movement  space  of  the  Cartesian 
Robot  to  be  a  1000  unit  cube  with  step  sizes  being  1  unit.  The  problem  with 
this  is  that  there  may  be  excessive  amount  of  points  generated  and  that 
there  may  be  too  much  accuracy  for  the  desired  job  resulting  in  costly 
computation  times  that  are  not  needed.  As  a  result  the  size  and  application 
of  the  Robot  need  to  be  considered  prior  to  designing  the  path  generating 
program. 

A  lot  of  areas  need  to  be  researched  further.  Among  those  are: 

1 .  The  affects  of  gravity  and  load  on  the  path  movement. 

2.  Perturbations  that  may  occur  due  to  shifting  loads  or  external  forces 
applied  to  the  robot. 

3.  Sensing  the  Robots  position  accurately. 
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4.  Obstacle  avoidance  with  not  only  the  tip  of  the  Robot  but  all  of  the 
joints  and  links. 

5.  Attachment  of  a  wrist  onto  the  Robot  Arm  and  the  algorithms  required 
to  move  the  arm  and  wrist  into  any  orientation  that  may  be  desired. 

6.  Generating  the  path  with  higher  ordered  approximations.  The  current 
path  generating  program  just  uses  linear  techniques  and  there  may  be 
higher  accuracies  with  more  sophisticated  techniques. 

7.  Using  more  accurate  techniques  for  the  dynamic  and  kinematic 
equations  of  motion.  The  lagrangian  may  not  supply  sufficient  accuracy 
for  higher  d.o.f.  than  3. 

8.  Optimize  the  program  so  that  the  program  operates  faster.  Use  of 
matrices  may  supply  more  speed.  Current  simulation  requires  high  cpu 
time  usage  and  may  prove  a  limiting  factor  for  higher  accuracy  or  more 
complex  path  movements.  For  the  Articulated  Robot  on  a  500  point  path 
the  cpu  time  usage  was  on  the  order  of  10's  of  minutes. 

9.  Build  and  demonstrate  the  feasibility  of  a  Robot  using  only  position  to 
give  the  desired  accuracy  results,  without  using  lead-through  teaching, 
manual  teaching,  or  complex  programming  techniques. 
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APPENDIX  A 
TRIGINOMETRY.  VECTORS.  AND  MATRICES 

This  appendix  contains  a  review  of  basic  trigonometry  and  matrix 
algebra.  Scalars  are  represented  by  lowercase  letters,  vectors  by 
lowercase  bold  letters,  and  matrices  by  uppercase  bold  letters. 


A.1  TRIGONOMETRY 

Right  angle  relationships 

h 


Sin  a  = 

r 

cos  a  = 

a 
r 

tan  a  = 

h 
a 

Law  of  Cosines 

2       u2 

a    =  b 

2 
+C 

-2bc(cosa) 

2  2       2 

b    =  a    +c  -2ac(cos  p) 

2  2        2 

c    =  a    +b  -2ab(cosY) 


h 


K- 


^ 
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A.2  MATRJCES 

Matrix  Multiplication 

A  is  an  n  X  m  matrix  with  components  [ay] 
i  =  1,2,..., n  and  j  =  1,2,0. .,m 

cA  =  Ac  =  [c  aij] 

a(A  +  B)  =  aA  +  aB 

(a  +  b)A  =  aA  +  bA 

a(bA)  =  (ab)A 

1(A)  =  A 

lA  =  Ai  =  A  where  I  is  the  identity  matrix  and  m  =  n 
Given  three  matrices  Amxn,  Bnxp,  and  Cmxp 

Amxn(Bnxp)  =  Cmxp  where  Cij  =  W|^    aik  bkj  ,  k=1 ,2,...  ,n 

(AB)C=A(BC) 
(A+B)C  =  AC  +  BC 
C(A  +  B)  =  CA  +  CB 

If  A  is  a  square  matrix  and  Aij  is  a  cofactor  in  |A| 

A  A  -1   =  A  ^   A  =  I 

A-""  =  [Aij]T/|A|  =    adjA/A 

(adj  A)  A  =  A  (  adj  A)  =  |A|  In 

A-""  =  1/|A| 
If  Ai  A2...An    is  the  product  of  square  matrices  then 

(AlA2...An)'^  =  An--'An-1-''...A2-''Al-1 
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If  AiA2...An  is  conformable  then 

(  AlA2...An  )T  =  (An)T(An-l)T  ..(A2)T(A1)T 

Translation  by  a  vector  ai  +  b]  +  c  k 


Trans  (a,b,c)  = 


10  0a 

0  10b 

00  1c 

000  1 

Rotation  about  the  X,  Y,  or  Z  axis 


Rot(x,e)  = 


Rot(y,e)  = 


10  0  0 

0  cose  -sine  0 

0  sine    cose  0 

0       0         0  1 

cose      0  sine  0 

0  10  0 

-sine     0  cose  0 

0  0       0  1 


Rot(z,e)  = 


cose  -sine  0    0 

sine    cose  0    0 

0  0  10 

0  0  0     1 
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Inverse  Transformation 


BIfcj      ^v  Qv  Mv 

rf'V             (^                (fSi  ^ 

fly      Uy  ^y  My 

"z  0^  a^  p^ 

0     0  0  1 


0     0     0 


"x    "y 

n^ 

-p-n 

--1 

Ox     Oy 

Oz 

-p-o 

T  = 

^x    9y 

^z 

-p-a 

Stretching  Transformation 


T  = 


a  0  0  0 

0  b  0  0 

0  0  c  0 

0  0  0  1 


Stretches: 

X  by  a 

Y  by  b 

Z  by  c 


Scaling  Transformation 


S  = 


s  0  0  0 

0  s  0  0 

0  0  s  0 

0  0  0  1 


Paul  (1981,  pp.  9-62)  or  Fu  (1987,  pp.  522-55)  have  good  sections  on  matrix 
operations  as  they  relate  to  Robotics. 
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APPENDIX  B 
DENAVIT-HARTENBERG  NOTATION 

B.1        DENAVIT-HARTENBERG  NOTATION 

Denavit  and  Hartenberg  (D-H)  proposed  a  system  designed  to  relate 
rotational  and  translational  matrices  between  adjacent  linkages.  Attaching 
a  coordinate  system  to  each  link  the  D-H  representation  results  in  a  4  x  4 
matrix  at  each  joint  and  4x4  matrices  used  to  transform  the  matrices 
between  coordinates.  In  this  way  the  end  effector  in  "tip  coordinates"  can 
be  expressed  in  terms  of  the  "base  coordinates"  by  sequentially  applying  the 
transformation  matrices.  Several  authors  (Paul,  pp.  41-63;  Fu,  pp.  36-41 ; 
Featherstone,  pp.  35-45;  and  Lee,  pp.  68-74)  go  into  some  detail  in  relating 
transformations  and  the  D-H  for  several  robot  models. 

In  Chapter  Three  motion  was  described  using  D-H  techniques  because 
ultimately  the  motion  of  each  joint  will  have  to  be  known  for  object 
avoidance.  Since  we  have  determined  where  we  want  the  tip  or  end  effector 
to  be,  it  would  be  more  convenient  to  use  D-H  matrices  to  reduce  all  of  the 
other  joint  positions.  Since  the  objective  of  this  thesis  was  to  follow  a 
path  without  regard  to  the  joint  positions,  other  than  the  end  effector,  we 
have  ignored  calculating  the  position  of  the  other  joints,  however,  the  D-H 
technique  was  used  to  generate  the  dynamic  equations  for  the  robot  arm 
simulation. 
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B.2       ROBOT  D-H  MATRICES 

There  are  two  types  of  joints,  prismatic  and  rotary,  that  need  to  be 
considered.  In  the  rotary  joint  the  joint  parameters  which  remain  constant 
are  d,  a,  a  while  9  changes  as  the  link  moves  with  respect  to  the  previous 
link  while  for  the  prismatic  joint  the  constant  joint  parameters  are  9,  a, 
and  a,  while  d  is  variable.  Figure  B.1  shows  an  example  of  rotary  and 
prismatic  motion  along  with  the  associated  4x4  matrix.  Two  common 
robots,  the  Stanford  Robot  and  the  PUMA  Robot,  have  been  analysed  in  detail 
by  Paul  (1981 ,  pp.  73-78),  Fu  (1987,  pp.  37-48),  Lee  (1982,  pp.  63-68),  and 
Lee  and  Zeigler  (1984,  pp.  695-705). 

B.3       ARTICULATED  ROBOT  D=H  MATRICES 

Figure  B.2  shows  the  link  coordinates  for  the  3  d.o.f.  articulated  robot 
arm  and  lists  the  link  parameters  and  ranges  of  motion.  The  matrices  are 
given  in  Figure  B.3. 
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PRISMATIC  JOINT 


An  = 


cose  -slnScosa 

sine  cosecosa 

0  sin  a 

0  0 


sinesin  a  0 

-cos  esin  a  0 

cosa  d 

0  1 


If  e  and   a  are  0.0  then 


An  = 


^777777777777 

In  General: 
where 


1 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

d 

0 

0 

0 

1 

a  is  the  twist  angle  (rotation  around  x) 

e  is  the  angle  rotation  around  z 

a  translation  along  rotated  x 

d  translation  along  z 

An  =  ROT(z,e)TRANS(0,0,d)TRANS(a,0,0)ROT(x,cO 


An  = 


CJ5 


cose  -sinOcosa  sinOsina 

sine  cosOcosa  -cosOsina 

0  sin  a  cosa 

0  0  0 


If 


acose 
asine 

d 

1 


then 


y77777777777/. 

ROTARY  JOINT 


e 

=  e 

a 

=  90 

d 

=   0.0 

a 

=   0.0 

_ 

cose  0 

sine 

0 

An  = 

sin  e    0 

-cose 

0 

0         1 

0 

0 

0         0 

0 

1 

Figure  B.I     Rotary  and  Prismatic  Joints. 
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Link  parameters 

for  3  d.o.f. 

Articulated  Robot 
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Figure  B.2    Link  Coordinates  and  Parameters  for 
the  Articulated  Robot  Model. 
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cos  01      0       sin  01         0 

°A,   = 

sin  01      0       -cos  01      0 
0            1           0             di 
0            0           0              1 

COS02      -sin02      0       a2cos02 

'A2  = 

sin  02     COS02        0       a2s1n82 
0               0               10 
0               0               0             1 

COS03      0        s1n03        0 

^A3  = 

sin  03      0        -cos  03      0 
0            1           0             d3 
0            0           0              1 

T3   =     Ai       A2      A3 

T  =    A6  specifies  the  position  and  orlentati 

on  of  the 

of  the  Robot  with  respect  to  the  ground.  The 

T  matrix 

Is  called  the  Arm  Matrix. 

T  _     O-j-      3j 

'  -      '3     'e 

Figure  B.3    Articulated  Robot  D-H  Matrices 
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APPENDIX  C 
CARTESIAN  ROBOT  DSL  PROGRAMS 

This  appendix  contains  the  Cartesian  Robot  DSL  programs  used  to 
model  the  Cartesian  Robot  Arm.  One  program  is  presented  with  four 
variations  that  were  used. 

R0BSIM2T.F0RTRAN  /  RS I M2T6. FORTRAN 

These  programs  are  identical  except  in  the  way  that  the  graphs  are 
plotted.  R0BSIM2T  plots  each  graph  on  a  seperate  page,  while  RSIM2T6 
plots  up  to  six  graphs  on  a  single  page.  Modifications  to  the  Robot 
Simulation  [Kalagarios]  program  allow  for  inputting  multiple  data  runs, 
including  zero  steps,  and  can  be  adapted  for  either  STEP  or  RAMP  Excitation 
as  desired.  Inputs  are  controlled  on  a  position  accuracy  basis. 

ROBSIM2V.FORTRAN  /  RSIM2V6.FORTRAN 

These  programs  are  identical  to  the  previously  mentioned  programs 
except  in  the  way  that  the  data  points  are  inputted.  Inputs  are  controlled 
on  a  time  basis  and  is  reflected  in  two  line  changes  of  the  R0BSIM2T 
program. 
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R0BSIM2T.F0RTRAN  LISTING 

TOF: 

C  STEVEN  G.  GOODWAY 

C  R0BSIM2T  FORTRAN 

C  NPS/WEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 

C  CARTESIAN  ROBOT  SIMULATION  {R0BSIM2T.F0RTRAN} 

C  THIS  PROGRAM  TAKES  A  THREE  TUPLE  OF  POINTS  {X.Y,Z)  FROM  A 

C  TABLE  OF  THREE  TUPLES  AND  APPUES  THEM  TO  SIMULATION  OF  A 

C  CARTESIAN  ROBOT.  THE  RESULTING  MOVEMENT  FROM  THE  SIMULATION 

C  IS  THE  EXPECTED  MOVEMENT  OF  THE  TIP  OR  ENDPOINT  OF  THE  ROBOT. 

C  THE  NEXT  THREE  TUPLE  OF  POINTS  ARE  INPUTTED  AFTER  THE  VALUE 

C  OF  THE  ERROR  APPUED  TO  ALL  OF  THE  MOTORS  HAS  DECREASED  TO 

C  BELOW  A  VALUE  CALLED  VALMIN.  THERE  IS  ONE  BASIC  ASSUMPTION 

C  THAT  WAS  MADE  WHEN  CONSTRUCTING  THE  ORIGINAL  THREE  TUPLE  TABLE 

C  AND  WAS  CHOSEN  TO  INSURE  THE  ACCURACY  WHEN  APPLYING  THE  POINTS 

C  TO  THE  SIMULATION. 

C 

C  ASSUMPTIONS: 

C 

C  1 .  THE  MAXIMUM  ERROR  DURING  ANY  MOVE  IS  LESS  THAN  OR  EQUAL 

C  TO  THE  DISTANCE  TRAVELED  IN  THAT  DIMENSION  DURING  THE 

0  STEP. 

0 
C  RLEDEFS  REQUIRED  FOR  PROPER  OPERATION  OF  THE  PROGRAM  ARE 

0  FILEDEF  02    DISK  PATHX     DATA 

0  FILEDEF  03    DISK  SCOMPX  DATA 

C 

0  GRAPHS  THAT  RESULT  ARE  SINGLE  PAGE  GRAPHS  {9  TOTAL) 

C  TIME  RESPONSE  (3) 

C  PLANE  PROJECTION  (3) 

C  PATH  ERROR  (3) 
C 

r^  Ik************************'************* 

C        '  SIMULATION  PROGRAM  FOR  CARTESIAN  ROBOT  * 

C 

C        MAIN  PROGRAM  AREA 

C        USER  PARAMETER  ADJUSTMENTS 

C 

PARAM  K=1 .0.KI  =0.6,K2=1  OOOO.O.KMI  =59.29,KM2=90.25,KM3=77.44 
PARAM  VSAT=150.0,M1=0.082,M2=0.041  ,M3=0.041.MM=0.186 
PARAM  J1  =0.033,J2=0.033.J3=0.033.R1  =0.91  ,R2=0.91  ,R3=0.91 
PARAM  KT1  =1 4.4,KT2=1 4.4,KT3=1 4.4,L1  =0.0001  ,L2=0.0001  ,L3=0.0001 
PARAM  BM1  =0.04297,BM2=0.04297,BM3=0.04297,RO=0.5,LOAD=0.0 
PARAM  KV1  =0.101 2,KV2=0.1 01 2,KV3=0.1 01 2.T=0.00025,VALMIN=0.001 
PARAM  TV1  =0.0075,TV2=0.0075,TV3=0.0075 

INTEGER  SW1  ,SW2,SW3,N1  ,N2,N3,J 

. 

K1:  CURVE  SCAUNGCONSTAMT 
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K2: 
KM: 
VSAT: 
K: 


AMPLIFIER  GAIN 

IDEAL(MODEL)  MOTOR  CONSTANT 
SATURATION  UMITS  OF  AMPLIFIER 
VELOCITY  LOOP  FEEDBACK  GAIN  (MODEL) 


XPOS.YPOS^POS:  COMMANDED  ENDPOim"  POSITION 

T:  SAMPUNG  INTERVAL 

VALMIN:  VALUE  OF  E  USED  TO  CAUSE  NEXT  STEP  TO  BE  INPUTTED. 

H:  NUMBER  OF  POINTS  ALREADY  INPUTTED 

J:  NUMBER  OF  POINTS  IN  PATH  (+1 ) 


INITIAL 

J=5 

H=0 

SW1=0 

SW2=0 

SW3=0 

N1=0 

N2=0 

N3=0 

RX=0.0 

RY=0.0 

R2=0.0 

C1=0.0 

C2=0.0 

C3=0.0 

X1DOT=0. 

X2DOT=0. 

X3DOT=0. 

TL1=0. 

TL2=0. 

TL3=0. 

CF-1 7R0 

CALL  OPEND  (J) 

CALLNEXPT    (RX.RY.RZ.CI  .C2,C3) 
DERIVATIVE 
NOSCRT 

A1  =SQRT(2.*KM1  *VSAT) 

A2=SQRT(2.*KM2*VSAT) 

A3=SQRT(2.*KM3*VSAT) 

REF1=RX*CF 

REF2=RY*CF 

REF3=RZ*CF 
SORT 

TM1  =M1  +M2+M3+2*MM+LOAD 

TM2=M2+LOAD 

TM3=M2+M3+MM+LOAD 

JM1=TMrR0 

JM2=TM2*RO 

JM3=TM3*RO 
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JT0T1=J1+JM1 

JTOT2=J2+JM2 

JTOT3=J3+JM3 

USE  FOR  STEP  EXCITATION 

RTH1=REFrSTEP(0.0) 

RTH2=REF2*STEP(0.0) 

RTH3=REF3*STEP(0.0) 

USE  FOR  RAMP  EXCITATION 

RTH1  =(REF1  /TV1  )*(TIME-RAMP(TV1 )) 

RTH2=(REF2/TV2)*(TIME-RAMP(TV2)) 

RTH3=(REF3/TV3)*(TIME-RAMP{TV3)) 

E1=RTH1-C1 

E2=RTH2-C2 

E3=RTH3-C3 
*•..**..*«.***..**«.«..* — ..* ««. — .». NOSORT 

IF  (E1  .LT.0.0)  X1  D0T=-A1  *K1  *SQRT(ABS(E1 )) 
IF  (E1  .GE.0.0)  X1  DOT=  A1  *K1  *SQRT(E1 ) 
IF  (E2.LT.0.0)  X2DOT=-A2*KrSQRT(ABS(E2)) 
IF  (E2.GE.0.0)  X2D0T=  A2*KrSQRT(E2) 
IF  (E3.LT.0.0)  X3DOT=-A3*KrSQRT{ABS(E3)) 
IF  (E3.GE.0.0)  X3D0T=  A3*KrSQRT(E3) 
*       USE  FOR  RAMP  OR  SINE  FUNCTIONS/  POSITION  METHOD 

IF  ((ABS(CX-RX).LT.VALMIN).AND.(ABS(CY-RY).LT.VALMIN)  ... 

.AND.(ABS(CZ-RZ).LT.VALMIN))  THEN 
IF  ((RTH1  .GE.REF1).AND.(RTH2.GE.REF2).AND.(RTH3.GE.REF3))THEN 

USE  FOR  STEP  FUNCTIONS/  POSITION  METHOD 
IF  ((ABS(E1).LT.VALMIN).AND.(ABS(E2).LT.VALMIN).AND. ... 

(ABS(E3).LT.VALMIN))  THEN 
CALLENDRUN 

USE  FOR  VELOCITY  METHOD    "*""****R0BSIM2V""' 
IF  (TIME.GE.((H+1  )*TSTEP))  THEN 
CALLENDRUN 
ENDIF 
ENDIF 
SORT 

KC1D0T=K*C1D0T 

X1  D0TE=X1  D0T-KC1  DOT 

V1  =LiMIT(-VSAT.VSAT,K2*X1  DOTE) 

ClDDT=VrKM1 

C1  DOT=INTGRL(0.0,C1  DDT) 

C1=INTGRL(0.0,C1DOT) 

CX=Cri7CF 

VM1=V1-KVrCR1D0T 

MP1=REALPL(0.0,L1/R1,VM1/R1) 

MT1=KTrMP1 

MT1  E=MT1  -BM1  *CR1  D0T-TL1 

CR1  DDT=(1  ./JT0T1  )*MT1  E 

CR1  DOT=INTGRL(0.0,CR1  DDT) 

CR1  =INTGRL(0.0,CR1  DOT) 
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CRx=CRriycF 

XXD0T=X1D0riyCF 
CXD0T=C1D0riyCF 
CRXD0T=CR1  DOn  VCF 


KC2DOT=K*C2DOT 

X2DOTE=X2DOT-KC2DOT 

V2=LIMIT(-VSAT,VSAT,K2*X2DOTE) 

C2DDT=V2*KM2 

C2DOT=INTGRL{0.0.C2DDT) 

C2=INTGRL(0.0,C2DOT) 

CY=C2*1 7CF 

VM2=V2-KV2*CR2DOT 

MP2=REALPL(0.0,L2/R2,VM2/R2) 

MT2=KT2*MP2 

MT2E=MT2-BM2*CR2DOT-TL2 

CR2DDT=(1  ./JTOT2)*MT2E 

CR2DOT=INTGRL(0.0,CR2DDT) 

CR2=INTGRL(0.0.CR2DOT) 

CRY=CR2*1  yCF 

XYD0T=X2D0ri  VCF 

CYD0T=C2D0T*iyCF 

CRYD0T=CR2D0ri  JCF 

KC3DOT=K*C3DOT 

X3DOTE=X3DOT-KC3DOT 

V3=LIMIT(-VSAT,VSAT,K2*X3DOTE) 

C3DDT=V3*KM3 

C3DOT=INTGRL(0.0,C3DDT) 

C3=INTGRL(0.0,C3DOT) 

CZ=C3*iyCF 

VM3=V3-KV3*CR3DOT 

MP3=REALPL(0.0,L3/R3,VM3/R3) 

MT3=KT3*MP3 

MT3E=MT3-BM3*CR3DOT-TL3 

CR3DDT={1  ./JTOT3)*MT3E 

CR3DOT=INTGRL{0.0,CR3DDT) 

CR3=INTGRL(0.0,CR3DOT) 

CR2=CR3*1  yCF 

XZD0T=X3D0riyCF 

CZD0T=C3D0ri  JCf 

CRZD0T=CR3D0T*1  yCF 

NOSORT 

IF(ABS(V1).LT.VALMIN)  GOTO  112 

IF(N1.EQ.0)GOTO111 

IF  (CI  D0T.GT.X1  DOT)  SW1  =1 

IF(SW1.EQ.1)GOT0  222 

KS1  =ABS(2.*CR1  )/(((N1  •T)"2)*V1 ) 

KM1=KS1 
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222  COn"INUE 

C1=CR1 
C1D0T=CR1D0T 

111  N1=N1+1 

112  COVfTINUE 

IF(ABS(V2).LT.VALMIN)  GOTO  334 
IF  {N2.EQ.0)  GOTO  333 
IF  (C2DOT.GT.X2DOT)  SW2=1 
IF(SW2.EQ.1)GOT0  444 
KS2=ABS(2.*CR2)/(((N2*T)**2)*V2) 
KM2=KS2 
444  (X>mNUE 

C2=CR2 
C2DOT=CR2DOT 

333  N2=N2+1 

334  COMTINUE 

* 

IF(ABS(V3).LT.VALMIN)  GOTO  556 
IF  (N3.EQ.0)  GOTO  555 
IF  (C3DOT.GT.X3DOT)  SW3=1 
IF(SW3.EQ.1)GOT0  666 
KS3=ABS(2.*CR3)/(((N3*T)"2)*V3) 
KM3=KS3 
666  CXKTINUE 

C3=CR3 
C3DOT=CR3CX)T 

555  N3=N3+1 

556  CXNTINUE 

SORT 

^***..*..***..*.*. «.*.*..•*..«.....**.•.•♦.. *  TERMINAL 

WRITE  (3,43)  RX,RY,RZ.CX.CY,CZ 
CALL  CERR(RX,RY,RZ,CX,CY,CZ,DC1  ,DC2,DC3) 
43  FORMATC  •.T2,6(F1 0.8,2X)) 

H=H+1 
IF(H.GE.J-1)THEN 

RETURN 
ENDIF 

CALL  NEXPT  (RX,RY.RZ.C1,C2.C3) 
CALLCONTIN 
METHOD  RKSFX 

CONTRL  FINTIM  =10.000,DELT=0.00005  ,DELS=0.00010 
PRINT  0.005  ,H,J,RX,RY,RZ.CX,CY,CZ 
SAVE     (S1 )  0.005,RX,RY,RZ.CX,CY.CZ,DC1  ,DC2,DC3 
GRAPH  (G1/S1,DE=TEK618)  TIME(LE=8.0,NI=8,UN='SECS*),... 

CX(NI=8,LO=-.5,SC=.25,UN='INCHES') 
GRAPH  (G2/S1,DE=TEK618)  TIME(LE=8.0,NI=8,UN='SECS').... 

CY(NI=8,LO=-.5,SC=.25,UN='INCHES') 
GRAPH  (G3/S1,DE=TEK618)  TIME(LE=8.0,NI=8,UN='SECS'),... 
CZ(NI=8,LO=-.5,SC=.25,UN='INCHES') 
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GRAPH  (G4/S2,DE=TEK618)  CX(LE=8.0,LO=-.25,NI=8,UN='INCHES'),... 

CY(NI=8,LO=-.25,SC=0.25,UN='INCHES') 
GRAPH  (G5/S2,DE=TEK618)  CX(LE=8.0.LO=-.25,NI=8,UN='INCHES'),... 

CZ(NI=8,LO=-.25,SC=0.25,UN='INCHES') 
GRAPH  (G6/S2,DE=TEK618)  CY{LE=8.0.LO=-.25,NI=8,UN-'INCHES'),... 

CZ(NI=8,LO=-.25,SC=0.25,UN='INCHES') 
GRAPH  (G7/S1.DE»TEK618)  TIME(UN='SECS').... 

DC1         (UN='ERROR(INCHES)') 
GRAPH  (G8/S1.DE=TEK618)  TIME(UN='SECS'),... 

DC2         (UN='ERROR(INCHES)') 
GRAPH  (G9/S1.DE=TEK618)  TIME(UN='SECS'),... 

DC3         (UN='ERROR(INCHES)') 
LABEL  (G1  ,G2,G3)    TIME  RESPONSE 
LABEL  (G4,G5.G6)     PLANE  PROJECTION 
LABEL  (G7,G8,G9)    PATH  ERROR 
BJD 
STOP 

C        FORTRAN  SUBROLTTINES 
FORTRAN 

C 

f^        ********************** 

C        *  SUBROUTINE  OPEND  * 

c 

C        TAKE  THE  VALUE  OF  J  OFF  THE  TABLE  TO  BEGIN  SETTING  UP  FOR 

C        INPUT  ROUTINE. 

C 

SUBROUTINE  OPEND  (J) 

REWIND  02 

READ  (2,  •)  J 
C  12  F0RMAT(I5) 

RETURN 

BJD 
C 

c 
c 

C        *  SUBROUTINE  NEXTP  * 
C       — ..— — * 

c 

C        INPUTS  THE  NEXT  POINT  WHEN  THE  VALUE  OF  E  DROPS  BELOW  VALMIN 
C        AND  CHECKS  TO  INSURE  THAT  ALL  OF  THE  VALUES  ARE  NOT  AT  ZERO. 
0 


SUBROUTINE  NEXPT(RX,RY,RZ,C1  ,C2,C3) 
67  READ  (02,  *)  RX.RY.RZ 

IF((RX.LE.O.O).AND.(RY.LE.O.O).AND.(RZ.LE.O.O).AND. 

(C1.LE.0.0).AND.(C2.LE.0.0).AND.(C3.LE.0.0))  THEN 

GOTO  67 
ENDIF 
RETURN 
mD 
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c 

C        •  SUBROUTINE  CERR  * 
C        """ — * 

c 

C        CHECKS  THE  ERROR  BETWEEN  THE  DESIRED  PATH  AND  THE  SIMULATED 

C        RESULT  IN  THREE  DIMENSIONS 

C 

SUBROUTINE  CERR(RX,RY,RZ.CX.CY,CZ.DC1  ,DC2,DC3) 

DC1=    (RX-CX) 

DC2=    (RY-CY) 

DC3=    (RZ-CZ) 

RETURN 

BJD 
0 

C        *  END  ROBOT  SIMULATION  SUBPROGRAM  * 
Q        * *.....**.*. . 

C 
EOR 
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GRAPH  MODIFICATIONS  TO  R0BSIM2T.F0RTRAN  USED  IN  ROBS2T6.FORTRAN. 
*     6  PLOTS  ON  FIRST  PAGE-TIME  RESPONSE  AND  PLANE  PROJECTIONS 

3  PATH  ERROR  ON  SECOND  PAGE 

XY  PLANE  PROJCTION  AND  3  PATH  ERRORS  ON  THIRD  PAGE 
GRAPH  (G1/S1,DE=TEK618,PO=1)  TIME(LE=5.0,NI=5,UN='SEC'),... 

CX(NI=2,LO=0.0,SC=0.5,UN='INCHES') 
GRAPH  {G2/S1  ,DE=TEK61 8,0V,P0=1 ,2.80)  TIME(LE=5.0,NI=5,UN='SEC'),... 

CY(NI=2,LO=0.0,SC=0.5.UN='INCHES') 
GRAPH  (G3/S1  ,DE=TEK61 8,0V,P0=1 .5.6)  TIME(LE=5.0,NI=5.UN='SEC'),... 

CZ(NI=2.LO=0.0,SC=0.5,UN='INCHES') 
GRAPH  (G4/S1,DE=TEK618,OV,PO=8)  CX(LE=2.0,LO=0.0,NI=2,... 

SC=0.5,UN='SEC'),  CY(NI=2,LO=0.0,SC=0.5,UN='INCHES*) 
GRAPH  (G5/S1,DE=TEK618,OV,PO=8,2.80)  CX(LE=2.0,LO=0.0,NI=2,... 

SC=0.5,UN='SEC'),CZ(NI=2,LO=0.0,SC=0.5,UN='INCHES*) 
GRAPH  (G6/S1,DE=TEK618,OV,PO=8,5.60)  CY(LE=2.0,LO=0.0,NI=2,... 

SC=0.5.UN='SEC').CZ(NI=2,LO=0.0,SC=0.5,UN=INCHES') 
GRAPH  (G7/S1,DE=TEK618,PO=1)  TIME(LE=5.0.NI=5.UN='SEC'),... 

DC1{NI=2  ,UN='INCHES') 

GRAPH  (G8/S1  ,DE=TEK61 8,0V.P0=1 ,2.80)  TIME(LE=5.0,NI=5,UN='SEC'),... 

DC2(NI=2  ,UN='INCHES') 

GRAPH  (G9/S1  ,DE=TEK61 8,0V,P0=1 ,5.6)  TIME(LE=5.0,NI=5,UN='SEC'),... 

DC3(NI=2  ,UN='INCHES') 

GRAPH  {G1 3/S1  ,DE=TEK61 8,P0=1 ,1 .75)  CX(LE=5.0,LO=0.00,NI=1 0,SC=.1 ,... 

UN='INCHES').  CY(NI=1 0,LO=0.00,LE=5.00.UN='INCHES',SC=.1 ) 
GRAPH  (G1 4/S1  ,DE=TEK61 8,OV,PO=7.5)  TIME(LE=5.0,NI=5,UN=*SEC').... 

DC1(NI=2  ,UN='ERR(IN)') 

GRAPH  (G15/S1  ,DE=TEK618,OV,PO=7.5,2.80)  TIME(LE=5.0,NI=5,UN=SEC'),., 

DC2(NI=2  ,UN=ERR(IN)') 

GRAPH  (G1 6/S1  .DE=TEK61 8.0V,PO=7.5,5.6)  TIME(LE=5.0,NI=5,UN='SEC'),... 

DC3{NI=2  ,UN='ERR(IN)') 

LABEL  (G1)  TIME  RESPONSE 
LABEL  (G4)  PLANE  PROJECTION 
LABEL  (G7)  PATH  ERROR 
LABEL(G10,G11,G12)  PATH  ERROR 
LABEL  (G1 3)  XY  PLANE  PROJECTION 
LABEL  (G14)  PATH  ERROR 
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APPENDIX  D 
ARTICULATED  ROBOT  DSL  PROGRAMS 

This  appendix  contains  the  Articulated  Robot  DSL  programs  used  to 
model  the  Articulated  Robot  Arm.  It  also  contains  the  results  of  the  single 
step  input  used  to  adjust  the  step  timing  for  the  RAMP  Excitations  and  the 
Velocity  Methods. 

RSIMREV1. FORTRAN 

This  program  adapts  the  Articulated  Robot  Arm  program  (Kalogiros, 
1987)  for  multiple  run  capability,  adjusts  for  zero  input,  and  includes  both 
STEP  and  RAMP  excitation  possibilities.  Designed  for  Position  Method 
control  it  also  takes  care  of  2n  to  0.0  movements.  It  does  not  adjust  for 
paths  whose  Cartesian  Coordinates  pass  through  (0.5,  0.5,  0.5)  which  is  the 
Articulated  Coordinate  System  "home"  position.  Inputted  values  are  a 
series  of  three  tuples  in  Articulated  Coordinates  (CP1  ,CP2,CP3)  which 
follow  a  path  length  integer. 

RSIMREV2.F0RTRAN 

This  program  is  the  Velocity  version  of  the  proceeding  program  and 
was  not  ran  to  verify  its  operation.  Modifications  are  supplied  here  only  for 
information  purposes  and  are  noted  in  the  RSIMREV1  program. 
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TCF: 

C  STB/EN  G.  GOODWAY 

C  RSIMREV1  .FORTRAN 

C  NPS/WEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 

C  REVOLUTE  ROBOT  SIMULATION 

C  THIS  PROGRAM  TAKES  THE  THREE  TUPLE  OF  ARTICULATED  COORDINATES 

C  THAT  DESCRIBES  THE  PATH  THAT  THE  ENDOINT  OF  A  ARTICULATED 

0  ROBOT  IS  TO  FOLLOW  AND  APPUES  THE  THREE  TUPLE  TO 

0  A  DSL  PROGRAM  FOR  EACH  ARM  AND  THE  CORRESPONDING  ARM 

0  MOVEMENT  IS  COMPAIRED  TO  THE  ORIGINAL  THREE  TUPLE  FOR 

0  ACCURACY. 

C 

C      FILEDEFS  REQUIRED  IN  'DSLGADD  EXEC  ARE 

C  FILEDEF  04  DISK  DAPATH  DATA  (RECFM  F 

0  FILEDEF  08  DISK  APATH4  DATA  (RECFM  F 
C 


C         *  SIMULATION  FOR  ARTICULATED  ROBOT  * 

C 

0         MAIN  PROGRAM  AREA 

C         USER  PARAMETER  ADJUSTMENTS 

C 

PARAM  K=1 .0,K1  =0.6,K2=1 0000,0,KM1  =0.4225,KM2=0.4225,KM3=4.0 

PARAM  VSAT=150.0,M1=0.268,M2=0.227,M3=0.041 

PARAM  J1=0.033,J2=0.033,J3=0.033,R1=0.91  ,R2=0.91  ,R3=0.91 

PARAM  KT1  =1 4.4.KT2=1 4.4,KT3=1 4.4,L1  =0.0001  ,L2=0.0001  .L3=0.0001 

PARAM  BM1  =0.04297,BM2=0.04297,BM3=0.04297,LOAD=0.0 

PARAM  KV1  =0.1 01 2,KV2=0.1 01 2,KV3=0.1 01 2,T=0.00025,VALMIN=0.001 

PARAM  TV1  =0.0075JV2=0.0075 JV3=0.0075 

PARAM  D1  =1 5.,D2=1 0.,D3=1 0.,G=386,4 

PARAM  REF1 =1  .,REF2=1  ..REF3=1 . 

INTEGER  SW1  .SW2,SW3,N1  ,N2,N3.J 

K1:  CURVE  SCAUNG  CONSTANT 

K2:  AMPLIFIER  GAIN 

KM:  IDEAL(MODEL)  MOTOR  CONSTANT 

VSAT:  SATURATION  UMITS  OF  AMPLIFIER 

K:  VELOCITY  LOOP  FEEDBACK  GAIN  (MODEL) 

CP1  .CP2,CP3:  COMMANDED  POSITION  IN  RADIANS 

T:  SAMPUNG  INTERVAL 

VALMIN:  VALUE  OF  E  USED  TO  CAUSE  THE  NEXT  STEP  TO  BE  INPUTTED 

H:  NUMBER  OF  POINTS  ALREADY  INPUTTED 

J:  NUMBER  OF  POINTS  IN  PATH  (+1) 

INITIAL 
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J=5 
H=0 
SW1=0 
SW2=0 
SW3=0 
DC1=0. 
DC2=0. 
DC3=0. 
N1=0 
N2=0 
N3=0 
C1  DT=0.0 
C2DT=0.0 
C3DT=0.0 
C1  DDT=0.0 
C2DDT=0.0 
C3DDT=0.0 
X1DOT=0. 
X2DOT=0. 
X3DOT=0. 
CR1=0. 
CR2=0. 
CR3=0. 
CR1DT=0. 
CR2DT=0. 
CR3DT=0. 
CR1DDT=0. 
CR2DDT=0. 
CR3DDT=0. 
TL1=0. 
TL2=0. 
TL3=0. 
MP1=0. 
MP2=0. 
MP3=0. 
MT1=0. 
MT2=0. 
MT3=0. 
CP1=0. 
CP2=0. 
CP3=0. 

CALL  OPEND  (J) 
CALLFHOME(C1,C2,C3) 
CALL  NEXPT{H,CP1  ,CP2,CP3,C1  ,C2,C3) 

CI1=CP1 

CI2=CP2 
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CI3=CP3 

A1=SQRT(2.*KMrVSAT) 

A2=SQRT(2.*KM2*VSAT) 

A3=SQRT(2/KM3*VSAT) 

DERIVATIVE 

RR1=CPrSTEP(0.0) 

RR2=CP2*STEP(0.0) 

RR3=CP3*STEP(0.0) 

E1=RR1-C1 

E2=RR2-C2 

E3=RR3-C3 
**...«...*.««*.....*.*.»*»**.«..*.«**^.****.*.. NOSORT 

D1 1     =M3*(D2*COS{CR2)+D3*COS(CR2+CR3))"2+M2*D2**2*(COS(CR2))" 
D1 1 2=2*((M2+M3)*D2"2*COS(CR2)*SIN(CR2)... 

+M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)... 

+M3*D2*D3*SIN(2*CR2+CR3)) 
D113=2*(M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)... 

+M3*D2*D3*COS(CR2)*SIN{CR2+CR3)) 
D22=(M2+M3)*D2**2+M3*D3**2+2*M3*D2*D3*SIN(CR3) 
D23  =M3*D3"2+M3*D2*D3*SIN(CR3) 
D21 1  =(M2+M3)*D2**2*COS(CR2)*SIN(CR2)... 

+M3*D3"2*COS(CR2+CR3)*SIN(CR2+CR3)... 

+M3*D2*D3*SIN(2*CR2+CR3) 
D223=2*M3*D2*D3*SIN(CR3) 
D233=  M3*D2*D3*SIN(CR3) 
D32  =  M3*D3"2+M3*D2*D3*COS(CR3) 
D33  =M3*D3"2 
D31 1  =M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)... 

+M3*D2*D3*COS(CR2)*SIN(CR2+CR3) 
D322=M3*D2'D3*SIN(CR3) 

G2  =(M2+M3)*G*D2*COS(CR2)+M3*G*D3*COS(CR2+CR3) 
G3  =M3*G*D3*COS(CR2+CR3) 
TL1  =  -D1 1 2*CR1  DT*CR2DT-D1 1 3*CR1  DrCR3DT 

Tl_2  =  D23*CR3DDT+D21  rCR1  DT**2-D33*CR3DT"2-D223*CR2DrCR3DT 
TL3  =  D32XR2DDT+D31 1 XRI  DT**2+D322*CR2DT**2 
JT0T1  =D11+J1 
JT0T2  =  D22+J2 
JT0T3  =  D33+J3 

IF  (E1  .LT.0.0)  X1  D0T=-A1  *K1  *SQRT(ABS(E1 )) 

IF  (E1  .GE.0.0)  X1  DOT=  A1  *K1  *SQRT(E1 ) 

IF  (E2.LT.0.0)  X2DOT=-A2*KrSQRT(ABS(E2)) 

IF  {E2.GE.0.0)  X2D0T=  A2*KrSQRT(E2) 

IF  {E3.LT.0.0)  X3DOT=-A3*KrSQRT(ABS(E3)) 

IF  (E3.GE.0.0)  X3D0T=  A3*KrSQRT(E3) 

USE  FOR  RAMP  OR  SINE  FUNCTIONS 
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IF  ((ABS(C1-CP1).LT.VALMIN).AND.(ABS(C2-CP2).LT.VALMIN) ... 
AND.{ABS{C3-CP3).LT.VALMIN))  THEN 
•       IF  ((RR1  .GE.CP1  ).AND.(RR2.GE.CP2).AND.(RR3.GE.CP3))THEN 
USE  FOR  STEP  FUNCTIONS/  POSITION  METHOD 
IF  ((ABS(E1).LT.VALMIN).AND.(ABS(E2).LT.VALMIN).AND. ... 

(ABS(E3).LT.VALMIN))  THEN 
CALLENDRUN 

USE  FOR  VELOCITY  METHOD  ****"""RSIMREV2* 
IF  (TIME.GE.({H+1  )*TSTEP))  THEN 
CALLENDRUN 
ENDIF 
ENDIF 
SORT 

KC1D0T=K*C1DT 
X1  D0TE=X1  D0T-KC1  DOT 
V1=LIMIT(-VSAT,VSAT,K2*X1  DOTE) 
C1DDT=VrKM1 
C1  DT=INTGRL(0.0,C1  DDT) 
C1=CI1  +INTGRL(0.0,C1  DT) 
VM1=V1-KVrCR1DT 
MP1=REALPL(0.0,L1/R1  ,VM1/R1) 
MT1=KTrMP1 
MT1  E=MT1 -BM1  *CR1  DT-TL1 
CR1  DDT=(1 7JT0T1  )*MT1  E 
CR1  DT=INTGRL(0.0,CR1  DDT) 
CR1=INTGRL(0.0.CR1DT) 

KC2DOT=K*C2DT 
X2DOTE=X2DOT-KC2DOT 
V2=LIMIT(-VSAT,VSAT,K2*X2DOTE) 
C2DDT=V2*KM2 
C2DT=INTGRL(0.0.C2DDT) 
C2=CI2  +  INTGRL{0.0,C2DT) 
VM2=V2-KV2*CR2DT 
MP2=REALPL(0.0,L2/R2,VM2/R2) 
MT2=KT2*MP2 
MT2E=MT2-BM2*CR2DT-TL2 
CR2DDT=(1 7JTOT2)*MT2E 
CR2DT=INTGRL(0.0,CR2DDT) 
CR2=INTGRL(0.0.CR2DT) 

KC3DOT=K*C3DT 
X3DOTE=X3DOT-KC3DOT 
V3=LIMIT(-VSAT,VSAT.K2*X3DOTE) 
C3DDT=V3*KM3 
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C3DT=INTGRL(0.0,C3DDT) 

C3=CI3  +  INTGRL(0.0,C3DT) 
VM3=V3-KV3*CR3DT 

MP3=REALPL(0.0.L3/R3,VM3/R3) 
MT3-KT3*MP3 

MT3E=;MT3BM3*CR3DT=TL3 
CR3DDT-(1  yjTOT3)*MT3E 

CR3DT=INTGRL(0.0.CR3DDT) 

CR3=INTGRL(0.0,CR3DT) 
********....**«***•*«****«*«*...«***...««♦*..*..***.*  SAMPLE 

NOeORT 

IF(ABS(V1).LT.VALMIN)  GOTO  112 
IF(N1.EQ.0)GOTO111 
IF  (C1 DT  .GT.X1  DOT)  SW1=1 
IF(SW1.EQ.1)GOT0  222 
KS1  =ABS(2.*CR1  )/(((N1  *T)"2)*V1 ) 
KMUKS1 
222  CONTINUE 

C1=CR1 
C1DT=CR1DT 

111  N1=N1+1 
CONTINUE 

112  CONTINUE 

IF(ABS(V2).LT.VALMIN)  GOTO  334 

IF  (N2.EQ.0)  GOTO  333 

IF  (C2DT  .GT.X2D0T)  SW2=1 

IF(SW2.EQ.1)GOT0  444 

KS2=ABS(2.*CR2)/(((N2*T)**2)*V2) 

KM2=KS2 
444  CONTINUE 

C2=CR2 

C2DT=CR2DT 

333  N2=N2+1 
CONTINUE 

334  CONTINUE 

IF(ABS(V3).LT.VALMIN)  GOTO  556 
IF  (N3.EQ.0)  GOTO  555 
IF  (C3DT  .GT.X3D0T)  SW3=1 
IF(SW3.EQ.1)GOT0  666 
KS3=ABS(2.*CR3)/(((N3*T)**2)*V3) 
KM3=KS3 
666  CONTINUE 

C3=CR3 
C3DT=CR3DT 
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555 

N3=N3+1 

CONTINUE 

556 

CONTINUE 

SORT 

*« .* — * — .... . ..............j^p^ll^^L 

CALL  CERR  (CP1  ,CP2,CP3,C1  ,C2,C3.DC1  ,DC2,DC3) 
WRITE  (4.43)     C1,C2.C3 
43  FORMAT(",T2,6(F10.8,2X)) 

H=H+1 
IF(H.GEJ-1)THEN 

RETURN 
ENDIF 

CALL  NEXPT{H,CP1  ,CP2,CP3,C1  ,C2.C3) 
CALL  PICR0(CP1  ,CP2,CP3,C1  ,C2,C3) 
CALLCONTIN 
METHOD  RKSFX 

CONTRL  FINTIM  =50.000,DELT=0.00005  ,DELS=0.00010 
PRINT  0.01 0,H  ,CP1,CP2,CP3.  C1,C2,C3 
SAVE     (S1 )  0.005,CP1  ,CP2,CP3  ,C1  ,C2,C3,DC1  ,DC2.DC3 
GRAPH  (G1/S1,DE=TEK618,PO=1)  TIME(LE=5.0,NI=5.UN='SECS'),... 

C1(NI=2,UN='RADS') 
GRAPH  (G2/S1,DE=TEK618,OV,PO=1 .2.80)  TIME(LE=5.0,NI=5.UN='SECS'),... 

C2(NI=2,UN=RADS') 
GRAPH  (G3/S1,DE=TEK61 8,0V,P0=1, 5.6)  TIME(LE=5.0,NI=5,UN='SECS'),... 

C3(NI=2,UN='RADS') 
GRAPH  (G4/S1,DE=TEK618,PO=1)  TIME(LE=5.0,NI=5,UN='SECS').... 

DC1  (NI=2,UN='ERR(RADS)') 
GRAPH  (G5/S1  ,DE=TEK61 8,0V,P0=1 ,2.80)  TIME(LE=:5.0.NI=5,UN='SECS'),... 

DC2(NI=2,UN='ERR(RADS)') 
GRAPH  (G6/S1. DE=TEK618,OV,PO=1, 5.6)  TIME(LE=5.0,NI=5.UN='SECS'),... 

DC3(NI=2,UN='ERR(RADS)') 
LABEL  (G1 )       TIME  RESPONSE 
LABEL  (G4)  PATH  ERROR 
BD 
STOP 

C         FORTRAN  SUBROUTINES 
FORTRAN 
C 
c        — *...-"• — * 

C         *  SUBROUTINE  OPEND  ' 
C         ***• """ 

c 

SUBROUTINE  OPEND  (J) 
REWIND  08 
READ  (08,  •)  J 
REWIND  04 
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WRITE(4,*)  J 
RETURN 
END 
C 

fs  ****************** 

C         *  SUBROUTINE  FHOME  * 

C 

SUBROUTINE  FH0ME(C1  ,C2,C3) 
READ(08,*)  C1  ,C2,C3 
RETURN 
END 
C 

Q  ****************** 

C         *  SUBROUTINE  NEXPT  * 

Q  ****************** 

c 

SUBROUTINE  NEXPT(H,CP1  ,CP2.CP3,C1  ,C2,C3) 

67  READ  (08,  *)  CP1  ,CP2,CP3 

IF  ((CP1.EQ.C1).AND.(CP2.EQ.C2).AND.(CP3.EQ.C3))  THEN 

GOTO  67 

ENDIF 

RETURN 

END 

C 

c        *— — — • 

C         •  SUBROUTINE  PICRO  * 
C        "*"""""*"" 

c 

C         CHECKS  FOR  2*PI  CROSSING 

SUBROUTINE  PICR0(CP1  .CP2.CP3.C1  .C2.C3) 

Pl=3.141529 

IF  ((C1.GT.PI).AND.(CP1.LT.(PI/2)))  THEN 
C1=2*PI-C1 

ENDIF 

RETURN 

END 
0 

Q  ***************** 

C         *  SUBROUTINE  CERR  * 
0         ***••."•*.•"♦** 

0 

SUBROUTINE  CERR{CP1  ,CP2.CP3,C1  .C2.C3,DC1  .DC2,DC3) 

DC1=(CP1-C1) 

DC2=(CP2-C2) 

DC3=(CP3-C3) 
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RETURN 
END 
C 

C  *  END  ROBOT  SIMULATION  SUBPROGRAM  ' 

c 

BDR 
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Figure  D.l     Articuloted  Robot,  1.0  radian.  STEP  Excited, 
CI,  C2,  and  C3  Step  Response. 
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Figure  D.2    Articulated  Robot.  1.0  radian.    STEP  Excitation, 
CI,  C2,  ond  C3  Phase  Response. 
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APPENDIX  E 
PATH  CONSTRUCTION  PROGRAMS  IN  FORTRAN 

This  appendix  contains  the  main  path  producing  program  and  subroutines. 
It  also  contains  all  of  the  path  modifications  for  each  of  the  motions 
conducted  in  this  thesis.  The  contents  of  this  chapter  are: 

E.1     ROBOT  PATH  PROGRAM  [ROBOPATH.FORTRAN] 

ROBOPATH  is  the  main  program  from  which  all  of  the  paths  are 
generated.  All  of  the  subroutines  are  listed  at  the  end  of  this  program. 

E.2  LINEAR  PATH  1  MAIN  PROGRAM 

E.3  LINEAR  PATH  2  MAIN  PROGRAM 

E.4  CIRCULAR  PATH  MAIN  PROGRAM 

E.5  HELIX  PATH  1  AND  2  MAIN  PROGRAM 

E.6  LINEAR  PATH  PROGRAM 

E.7  SINUSOIDAL  PATH  MAIN  PROGRAM 
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E.1   ROBOT  PATH  MAIN  PROGRAM 


TOR 

C  STEVEN  a  GOODWAY 

C  RCeDPATH.FOITrRAN 

C  NPS/WEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 

C  VERSION:: 

C  RPATH3-  LINEAR  CARTESIAN-  MODIFIED  FROM  VERSION 

C  R0BPATH7  FORTRAN-PARAMETRIC  SUBROUTINE  ADDED 

C  CARTESIAN  UNEAR  MOTION 

C  NPSM^EAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 

C  CARTESIAN  ROBOT  SIMULATION  {R0BSIM2S.F0RTRAN} 

C  THIS  PROGRAM  CALCULATES  THE  THREE  TUPLE  OF  POINTS  (X.Y^ 

C  THAT  DESCRIBES  THE  PATH  THAT  THE  ENDPOINT  OF  A  CARTESIAN 

C  ROBOT  IS  TO  FOLLOW.  THEN  THE  THREE  TUPLE  IS  APPUED  TO 

C  A  DSL  PROGRAM  FOR  EACH  ARM  AND  THE  CORRESPONDING  ARM 

C  MOVEMENT  IS  COMPAIRED  TO  THE  ORIGINAL  THREE  TUPLE  FOR 

C  ACCURACY. 
C 

C  ASSUMPTIONS: 
C 

C  1 .  THE  MAXIMUM  ERROR  DURING  ANY  MOVE  IS  LESS  THAN  OR  EQUAL 

C  TO  THE  DISTANCE  TRAVELED  IN  THAT  DIMENSION  DURING  THE 

C  STEP. 

C  2.  THE  INPUTED  EQUATIONS  ARE  OF  THE  FORM  Y=F(X)  AND  Z=F(X,Y). 
C 
C 

C  *  USER  MODIFICATION  AREA  * 


FUNCTION  EQUAT1(X,Y) 
EQUAT1  =  X 
RETURN 
END 

XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQAT1Y(X) 
EQAT1Y=(1.5-X) 
RETURN 
END 

EQUATION  2 

FUNCTION  EQUAT2(X.Y) 
EQUAT2=1.0 
RETURN 
END 
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XY  PROJECTION  FOR  EQUAT2 
FUNCTION  EQAT2Y(X) 

EQAT2Y=1.5-X 

RETURN 
END 
EQUATIONS 
FUNCTION  EQUAT3{X,Y) 

EQUAT3=1 .0 

RETURN 
END 

XY  PROJECTION  FOR  EQUAT3 
FUNCTION  EQAT3Y(X) 

EQAT3Y=1 .5-X 

RETURN 
END 
EQUATION  4 
FUNCTION  EQUAT4(X,Y) 

EQUAT4=1.0 

RETURN 
END 

XY  PROJECTION  FOR  EQUAT4 
FUNCTION  EQAT4Y{X) 

EQAT4Y=1 .5-X 

RETURN 
END 

PARAMETRIC  X  EQUATION  1 
FUNCTION  PEQX1(T) 

PEQX1=0.75 

RETURN 
END 

PARAMETRIC  Y  EQUATION  1 
FUNCTION  PEQY1(T) 

PEQY1={.5+.25*T) 

RETURN 
END 

PARAMETRIC  Z  EQUATION  1 
FUNCTION  PEQZ1(T) 

PEQZ1=0.00 

RETURN 
END 

PARAMETRIC  X  EQUATION  2 
FUNCTION  PEQX2(T) 

PEQX2=  (0.75-0.5*T) 

RETURN 
END 
PARAMETRIC  Y  EQUATION  2 
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FUNCTION  PEQY2(T) 
PEQY2=  0.75 
RETURN 
END 
C         PARAMETRIC  Z  EQUATION  2 
FUNCTION  PE0Z2(T) 
PEQZ2=  0.25 
RETURN 
END 
C         PARAMETRIC  X  EQUATION  3 
FUNCTION  PEQX3(T) 
PEQX3=  0.25 
RETURN 
END 
C         PARAMETRIC  Y  EQUATION  3 
FUNCTION  PEQY3(T) 

PEQY3=  0.75-.5*T 
RETURN 
END 
C         PARAMETRIC  Z  EQUATION  3 
FUNCTION  PEQZ3(T) 
PEQZ3=  0.0 
RETURN 
END 
C         PARAMETRIC  X  EQUATION  4 
FUNCTION  PEQX4(T) 
PEQX4=  0.25 
RETURN 
END 
C         PARAMETRIC  Y  EQUATION  4 
FUNCTION  PEQY4(T) 

PEQY4=0.25+0.75*T 
RETURN 
END 
C         PARAMETRIC  Z  EQUATION  4 
FUNCTION  PEQZ4{T) 

PEQZ4=1.0 
RETURN 
END 
C         END  OF  FUNCTION  DESCRIPTION  AREA 
C 

C         MAIN  PROGRAM  AREA 
C         USER  PARAMETER  ADJUSTMENTS 
DIMENSION  HSP0S(1 000,3) 

REALMAXOD,DELT,MAXODX,MAXODY.MAXODZ,MAXVX,MAXVY,MAXVZ 
COMMON  J.I.K.P 
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INTEGER  N 

DELT=.1 

MAXOD=.05 

Pl=3.141529 
ACC=1.0 
C         MASS  AND  INERTIAL  ADJUSTMENTS 

MAXVX=1  /MAXOD/DELT 

MAXVY=1  .*MAXOD/DELT 

MAXVZ=1  .*MAXOD/DELT 

MAXODX=MAXVX*DELT 

MAXODY=MAX\rrDELT 

MAXODZ=MAXVZ*DELT 
C         MODIFY  DELT  FOR  PARAMETRIC 

DELT=0.1 
C         MIN  AND  MAX  LIMITS 

VALMAX=1  E7 

VALMIN=1E-10 
C 

C         *  EQUATION  BOUNDS  AREA   * 

C 

C         FIRST  PATH  EQUATION 

C         START/FINISH  POINTS 

XS5=0.5 

XF5=1.00 
C         CALCULATE  Y  START/FINISH  POINTS 

YS5=EQAT1Y(XS5) 

YF5=EQAT1Y(XF5) 
C         CALCULATE  Z  START/FINISH  POINTS 

ZS5=EQUAT1(XS5,YS5) 

ZF5=EQUAT1(XF5.YF5) 
C 

C         SECOND  PATH  EQUATION 
C         START/FINISH  POINTS 
C         XS2=0.5 
C         XF2=1 .0 

C         CALCULATE  Y  START/FINISH  POINTS 
C         YS2=EQAT2Y(XS2) 
C         YF2=EQAT2Y(XF2) 
C         CALCULATE  Z  START/FINISH  POINTS 
C         ZS2=EQUAT2(XS2,YS2) 
C         ZF2=EQUAT2(XF2,YF2) 
C 

C         PARAMETRIC  START  AND  FINISH  POINTS 
C         FIRST  PARAMETRIC  EQUATION 
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TS=0.0 

TF=1 .00 

XS1=PEQX1{TS) 

YS1=PEQY1(TS) 

ZS1=PEQZ1(TS) 

XF1=PEQX1(TF) 

YF1=PEQY1(TF) 

ZF1=PEQZ1(TF) 
C 
C         SECX)ND  PARAMETRIC  EQUATION 

XS2=PEQX2(TS) 

YS2=PEQY2(TS) 

ZS2=PEQZ2{TS) 

XF2=PEQX2(TF) 

YF2=PEQY2(TF) 

ZF2=PEQZ2(TF) 
C 
C         THIRD  PARAMETRIC  EQUATION 

XS3=PEQX3(TS) 

YS3=PEQY3{TS) 

ZS3=PEQZ3(TS) 

XF3=PEQX3(TF) 

YF3=PEQY3(TF) 

ZF3=PEQZ3(TF) 
C 
C         FOURTH  PARAMETRIC  EQUATION 

XS4=PEQX4(TS) 

YS4=PEQY4(TS) 

ZS4=PEQZ4{TS) 

XF4=PEQX4(TF) 

YF4=PEQY4(TF) 

ZF4=PEQZ4(TF) 
C 

C         OUTPUT  START  AND  FINISH  POINTS 
C 

WRITE  (9,65)  '1  '.XSI  ,YS1  ,ZS1  ,XF1  ,YF1  ,ZF1 

WRITE  (6,65)  '1  ',XS1  ,YS1  .ZS1  ,XF1  ,YF1  ,ZF1 
C 

WRITE  (9,65)  •2',XS2,YS2,ZS2,XF2,YF2,ZF2 

WRITE  (6,65)  '2',XS2,YS2,ZS2,XF2,YF2,ZF2 
C 

WRITE  (9,65)  '3',XS3,YS3,ZS3,XF3,YF3,ZF3 

WRITE  (6,65)  •3',XS3,YS3,ZS3,XF3,YF3,ZF3 
C 

WRITE  (9,65)  •4',XS4,YS4,ZS4.XF4,YF4,ZF4 

WRITE  (6,65)  '4',XS4,YS4,ZS4,XF4,YF4,ZF4 
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c 


WRITE  (9,65)  ■5',XS5,YS5,ZS5,XF5.YF5,ZF5 
WRITE  (6,65)  '5',XS5,YS5,ZS5,XF5,YF5,ZF5 


*♦«♦****««***««♦*****«*** 


C         •  CALCULATE  PATH  MOVEMENT  * 
f\         «♦**♦*♦«*****•*«**«**«*** 

C 
C 

C         HOME 
J=1 
K=1 

HSPOS(1,1)=0. 
HSP0S(1 ,2)=0. 
HSPOS(1,3)=0. 

WRITE(02,1 81 )  HSP0S(1 ,1  ),HSP0S(1 ,2),HSP0S(1 ,3) 
1 81  FORMATC  ',T3,3(F1 0.8,2X)) 

C  PATH  FORMATION 
XS=XS1 
YS=YS1 
ZS=ZS1 
XF=XF1 
YF=YF1 
ZF-ZF1 
C         HOME  TO  START 

CALLHSTAR(XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
C 

C         *  START  TO  FINISH  EQUAT1  * 

c 

C         ADJUST  FOR  SLOW  SPEED 
C         MAXODX=MAXODX/ACC 
C         MAXODY=MAXODY/ACC 
C         MAXODZ=MAXODZ/ACC 

N=1 
C         CALL  STARF(XS,YS,ZS,XF,YF.ZF.N,MAXODX,MAXODY,MAXODZ,VALMIN 
C     *  .J,K) 

C 
C         START  TO  FINISH  PARAMETRIC  EQUATION  1 

CALL  SFPAR(N,TS,TF,DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN) 
C         LINEAR  FINISH  EQUAT1  TO  START  EQUAT2-FAST 
C         MAXODX=ACC*MAXODX 
C         MAXODY=ACC*MAXODY 
C         MAXODZ=ACC*MAXODZ 

XS=XF1 
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YS=YF1 
ZS=ZF1 
XF=XS2 
YF=YS2 
ZF=ZS2 

CALLFSTAR(XS,YS,ZS.XF,YF,ZF,MAXODX.MAXODY.MAXODZ,VALMIN,J.K) 
C 

/>  A********************** 

C         •  START  TO  FINISH  EQUAT2  * 
Q         .............. — **« 

C 

C  MAXODX=MAXODX'ACC 
C  MAXODY=MAXODY/ACC 
C         MAXODZ=MAXODZ'ACC 

XS=XS2 

YS=YS2 

ZS=ZS2 

XF=XF2 

YF=YF2 

ZF=ZF2 

N=2 
C         CALLSTARF(XS,YS^S,XF.YF,ZF.N.MAXODX,MAXODY.MAXODZ.VALMIN 
C     •  .J.K) 

C         START  TO  FINISH  PARAMETRIC  EQUATION  2 

CALLSFPAR(N,TS,TF,DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN) 
C 

C         LINEAR  FINISH  EQUAT2  TO  START  EQUAT3-FAST 
C         MAXODX=ACC*MAXODX 
C         MAXODY=ACC*MAXODY 
C         MAXODZ=ACC*MAXODZ 

XS=XF2 

YS=YF2 

ZS=ZF2 

XF=XS3 

YF=YS3 

ZF=ZS3 

CALLFSTAR(XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
C 

C         *  START  TO  FINISH  EQUAT3  * 

Q 

c 

c 

C  MAXODX=MAXODX/ACC 
C  MAXODY=MAXODY/ACC 
C         MAXODZ=MAXODZ/ACC 
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XS=XS3 

YS=YS3 

2S=ZS3 

XF=XF3 

YF=YF3 

ZF=ZF3 

N=3 
C         CALL  STARF(XS.YS,ZS,XF,YF7F,N,MAX0DX,MAX0DY,MAX0DZ,VALMIN 
C     •  .J.K) 

C         START  TO  FINISH  PARAMETRIC  EQUATION  3 

CALLSFPAR(N,TS,TF,DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN) 
C         UNEAR  FINISH  EQUAT3  TO  START  EQUAT4-FAST 
C         MAXODX=ACC*MAXODX 
C         MAXODY=ACC*MAXODY 
C         MAXODZ=ACC*MAXODZ 

XS=XF3 

YS=YF3 

ZS=ZF3 

XF=XS4 

YF=YS4 

ZF=ZS4 

CALLFSTAR(XS,YS,ZS^F,YF^F,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 


C         *  START  TO  FINISH  EQUAT4  * 

c 

XS=XF4 

YS=YF4 

ZS=ZF4 

XF=XS4 

YF=YS4 

ZF=ZS4 
C         ADJUST  FOR  SLOW  SPEED 
C         MAXODX=MAXODX/ACC 
C         MAXODY=MAXODY/ACC 
C         MAXODZ=MAXODZ/ACC 

N=4 
C         CALL  STARF(XS,YS,ZS.XF,YF,ZF,N,MAXODX,MAXODY,MAXODZ,VALMIN 
C     *  J.K) 

C 
C         START  TO  FINISH  PARAMETRIC  EQUATION  4 

CALLSFPAR(N,TS,TF.DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN) 
C         UNEAR  FINISH  EQUAT4  TO  START  EQUAT5-FAST 
C         MAXODX=ACC*MAXODX 
C         MAXODY=ACC*MAXODY 
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C         MAXODZ=ACC*MAXODZ 

XS=XF4 

YS=YF4 

ZS=ZF4 

XF=XS5 

YF=YS5 

ZF=ZS5 

CALLFSTAR(XS,YS.ZS,XF,YF,ZF,MAXODX,MAXODY.MAXODZ.VALMIN,J.K) 

C 

Q         .*.*. ..* — 

C         •  START  TO  FINISH  EQUAT5  * 
Q         «* — * *.«**. 

C 

XS=XS5 

YS=YS5 

ZS=ZS5 

XF=XF5 

YF=YF5 

ZF=ZF5 
C         ADJUST  FOR  SLOW  SPEED 
C         MAXODX=MAXODX/ACC 
C         MAXODY=MAXODY/ACC 
C         MAXODZ=MAXODZ^ACX) 

N=1 

CALLSTARFp<S,YS;ZS,XF,YF,ZF,N.MAXODX,MAXODY,MAXODZ,VALMIN 
.J.K) 
C 

C         START  TO  FINISH  PARAMETRIC  EQUATION  5 

C         CALL  SFPAR(N,TS.TF,DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN) 
C 

C         ADJUST  FOR  FAST  MOVEMENT 
C         MAXODX=ACC*MAXODX 
C         MAXODY=ACC*MAXODY 
C         MAXODZ=ACC*MAXODZ 
C         FINISH  EQUAT5  TO  HOME 

XF=XF5 

YF=YF5 

ZF=ZF5 

CALLFHOME(XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
C 

C         *  FINISH  POINT  FORMATION  * 

c 
c 

C         PRINTOUT  START  AND  FINISH  POINTS 
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WRITE(6,123) 
123  FORMAT('r) 

PRINT  */  • 

PRINT  *.'ROBOT  SIMULATION  3' 
PRINT  *,'  • 

PRINT  *;START  AND  FINISH  POINTS' 
C         COLUMN  HEADERS 

WRITE  (9.45)  'NO.'.'X-START  '/Y-START  '.'Z-START  '.'X-FINISH'. 

•Y-FINISH','Z-FINISH' 
WRITE  (6.45)  'NO.'.'X-START  '.'Y-START  '.'Z-START  '.'X-FINISH', 
'Y-FINISH'.'Z-FINISH' 
45  FORMAT  ('  ',T2.A3.T6,6(A8.4X)) 

WRITE  (9,65)  '1',XS1.YS1.ZS1.XF1.YF1.ZF1 
WRITE  (6.65)  '1  ',XS1  .YS1  .ZS1  ,XF1  .YF1  ,ZF1 
65  FORMAT  ('  •,T2,A3.6(2X.F1 0.8  )) 

C  READ  DATA 
REWIND  02 
DO  555  1=1  ,J+1 

READ(2.*)HSPOS(M),HSPOS(I,2),HSPOS(l.3) 
555  a>JnNUE 

C         FORMAT  DATA  FOR  DSL 
REWIND  02 
WRITE  (2,*)  J+1 
D0  553I=1,J+1 

WRITE(2,192)HSPOS(l.1).HSPOS(l,2).HSPOS(l.3) 
553  CONTINUE 

1 92  FORMATC  ',T3.3(F1 0.8.2X)) 

C         OUTPUT  VALUES  FOR  THREE  TUPLE 
PRINT  *,' ' 
PRINT  *,' ' 

PRINT  '.THREE  TUPLE  OF  POSITION  POINTS' 
WRITE(9.75)'X-POS','Y-POS','Z-POS' 
WRITE(6.75)'X-POS'.'Y-POS','Z-POS' 
75  FORMAT  ('  ',T3.3(A5.7X)) 

DO  85  1=1, J 

WRITE(9,95)  HSPOS(l,1),HSPOS(l,2),HSPOS(l.3) 
WRITE(6.95)  HSP0S(I.1  ).HSPOS(I.2).HSPOS(l.3) 
85  COIvmNUE 

95  FORMAT  ('  •.T2.3(F1 0.7.2X)) 

PRINT  •,' ' 

PRINT  *.'COMPLETED  POSITION  CALCULATION' 
PRINT  •.'COMMENCING  ROBOT  ARM  SIMULATION' 
PRINT  V ' 
END 
C 

C       "**" * 
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C         *  SUBROUTINE  LISTINGS  * 

c 

SUBROUTINE  HSTAR(XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY, 
MAXODZ.VALMIN.J.K) 
C         CALC  UNEAR  HOME  TO  START  MAXIMIZING  STEEPEST  PATH 
DIMENSION  HSP0S(1 000,3) 
REAL  MAXODX,MAXODY,MAXODZ,VALMIN 
REWIND  02 

WRITE(9,*)  'HOME  TO  START 
WRITE(9.*)  XS,YS,ZS 
WRITE(9,*)  XF,YF,ZF 
C         WRITE(9.*)  J.K 
DO  767  1=1  ,J 

READ  (02,*)  (HSP0S(I.L).L=1,3) 
767  COI^NUE 

DO10J=1,1000 

DELX=MAXODX 
DELY=MAXODY 
DELZ=MAXODZ 
C         WRITE  (9,*)  DELX,DELY,DELZ 
C         START  POINT  IS  AT  ZERO 

IF  ((XS.LT.VALMIN).AND.{YS.LT.VALMIN).AND.(ZS.LT.VALMIN))  THEN 
GOT0 100 
ENDIF 
C 
C         SCALE 

IF  (ZS.GT.XS)  THEN 

GOTO  20 
ENDIF 
IF(YS.GT.XS)THEN 

DELX=DELX*(XSA'S) 
DELZ=DELZ*(ZSA'S) 
ELSE 

DELY=DELY*(YS/XS) 
DELZ=DELZ*(ZS/XS) 
ENDIF 
GOTO  30 
20  IF  (ZS.GT.YS)  THEN 

DELX=DELX*(XS/ZS) 
DELY=DELY*{YS/ZS) 
ELSE 

DELX=DELX*(XSA'S) 
DELZ=DELZ*(ZSA'S) 
ENDIF 
30  HSP0S(J+1,1)=HSP0S(J,1)+DELX 
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HSP0S(J+1 .2)=HSPOS(J.2)+DELY 
HSP0S(J+1 ,3)=HSPOS(J.3)+DELZ 

WRITE(9.*)  HSP0S(J+1 ,1  ).HSP0S{J+1 ,2),HSP0S(J+1 ,3) 
C         CHECK  FOR  CX)MPLEnON 

IF((HSP0S(J+1 .1).GE.XS).AND.(HSP0S{J+1 ,2).GE.YS).AND„ 
(HSP0S(J+1 .3).GE.ZS))  THEN 
GOT0 100 
ENDIF 
C         CHECK  FOR  PLANE  MOVEMENfT 

IF  ((HSP0S(J+1.1),GT.XS).AND.(HSP0S(J,1).LT.XS))  THEN 

HSP0S(J+1 ,1  )=HSP0S{J.1 ) 
ENDIF 
IF  ((HSPOS(J+1.2).GT.YS).AND.(HSPOS(J,2).LT.YS))  THEN 

HSP0S(J+1 ,2)=HSPOS(J,2) 
ENDIF 
IF  ((HSPOS(J+1,3).GT.ZS).AND.(HSPOS(J.3).LT.ZS))  THEN 

HSP0S(J+1 ,3)=HSPOS(J.3) 
ENDIF 
10  CONTINUE 

100  HSP0S(J+1,1)=XS 

HSP0S{J+1,2)=YS 
HSP0S(J+1 .3)=ZS 
REWIND  02 
D0143I=1,J+1 

WRITE(2.182)HSPOS(l,1),HSPOS(l.2).HSPOS(I,3) 
143  CCHvmNUE 

1 82  FORMATC  ',T3,3(F1 0.8,2X)) 

RETURN 
END 
C 
C 

SUBROUTINE  STARF(XS,YS^S,XF,YF,ZF,N, 

MAXODX.MAXODY.MAXODZ.VALMIN.J.K) 
C         CALCULATE  EQUATIONS  FROM  START  TO  FINISH 
DIMENSION  HSP0S(1 000,3) 
REAL  MAXODX.MAXODY,MAXODZ,VALMIN 
REWIND  02 

WRITE(9,*)  'START  TO  FINISH' 
D0  768I=1,J+1 

READ  (02,*)  (HSP0S{I,L),L=1,3) 
768  CONTINUE 

DO  11  K=J+1,1000 
DEI_X=MAXODX 
DELY=MAXODY 
DELZ=MAXODZ 
0 
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C         CHECK  FOR  NEGATIVE  VALUE  DELX.DELY,  OR  DELZ 
IF  ((XF-XS).LT.O.O)  THEN 

DELX=-DELX 
ENDIF 
C         ADJUST  FOR  X  PLANE  MOVEMENT 

IF  (ABS(HSP0S(K,1)-XF).LE.MAX0DX)  THEN 

DELX=(ABS(HSP0S(K,1)-XF)*DELX)/(ABS(DELX)) 
ENDIF 
C         CALCULATE  DELY  BASED  ON  DELX  OF  MAXODX 
IF(N.EQ.1)THEN 

DELY=EQAT1  Y(HSP0S(K,1  )+DELX)-HSP0S(K,2) 
ELSEIF  (N.EQ.2)  THEN 

DELY=EQAT2Y(HSPOS{K,1)+DELX)-HSPOS(K,2) 
ELSEIF  (N.EQ.3)  THEN 

DELY=EQAT3Y{HSPOS(K,1)+DELX)-HSPOS(K,2) 
ELSEIF  (N.EQ.4)  THEN 

DELY=EQAT4Y(HSPOS(K.1)+DELX)-HSPOS(K,2) 
ENDIF 
C         ADJUST  FOR  PLANE  MOVEMENT  IN  Y  PLANE 
IF  (ABS(DELY).LE.VALMIN)  THEN 

GOTO  51 2 
ENDIF 
C         ADJUST  FOR  DELY  GREATER  THAN  MAXODY 
WHILE  (ABS(DELY).GT.MAXODY)  DO 

DELX=DELX*(MAXODY/(ABS(DELY))) 
C         WRITE  (9,*)  DELX,DELY,HSP0S(K,1  ).HSP0S(K,2) 
C         ADJUST  FOR  PLANE  MOVEMENT  IN  X  DIRECTION 
IF  (ABS(DELX).LE.VALMIN)  THEN 
DELX=0.0 
IF  (ABS(HSP0S{K,2)-YF).GT.MAX0DY)  THEN 
DELY=(DELY*MAXODY)/(ABS(DELY)) 
ELSE 

DELY=(DELY*ABS(HSP0S(K,2)-YF))/(ABS(DELY)) 
ENDIF 

WRITE  (9,*)  DELX.DELY 
GOTO  51 2 
ENDIF 
IF(N.EQ.1)THEN 

DELY=EQAT1  Y(HSP0S(K,1  )+DELX)-HSP0S{K,2) 
ELSEIF  (N.EQ.2)  THEN 

DELY=EQAT2Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 
ELSEIF  (N.EQ.3)  THEN 

DELY=EQAT3Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 
ELSEIF  (N.EQ.4)  THEN 

DELY=EQAT4Y(HSPOS(K,1)+DELX)-HSPOS(K,2) 
ENDIF 
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END  WHILE 
C         IF  ((YF-YS).LT.O)  THEN 
C        DELY=-DELY 
C         ENDIF 
C         CALCULATE  DELZ 
512  IF(N.EQ.1)THEN 

DELZ=    {EQUAT1  (HSP0S(K.1  )+DELX.HSPOS(K.2)+DELY)-HSPOS(K,3)) 
ELSEIF  (N.EQ.2)  THEN 

DELZ=    (EQUAT2(HSP0S(K,1  )+DELX,HSPOS(K.2)+DELY)-HSPOS(K,3)) 
ELSEIF  (N.EQ.3)  THEN 

DELZ=    (EQUAT3(HSP0S(K,1  )+DELX,HSPOS(K.2)+DELY)-HSPOS(K,3)) 
ELSEIF  (N.EQ.4)  THEN 

DELZ=    (EQUAT4(HSP0S(K,1  )+DELX,HSPOS(K,2)+DELY)-HSPOS(K,3)) 
ENDIF 
C         SCALE  FOR  MAXIMUM  MOVEMENT 
DX=ABS(DELX) 
DY=ABS(DELY) 
DZ=ABS(DELZ) 
C         WRITE  (9,*)  DX.DY.DZ 

IF  ((DZ.GT.DX).AND.(DZ.GT.DY))  THEN 
DELX=DELX*MAXODX/DZ 
DELY=DELY*MAXODY/DZ 
DELZ=DELZ*MAXODZ/DZ 
ENDIF 
C         WRITE  (9.*)'    •  .DELX.DELY.DELZ 
C         NEXT  POINT 

HSP0S{K+1 ,1  )=HSP0S(K,1  )+DELX 
HSP0S(K+1 ,2)=HSPOS(K,2)+DELY 
HSP0S(K+1 ,3)=HSPOS(K,3)+DELZ 

WRITE(9.183)HSPOS{K  ,1),HSP0S(K  ,2).HSP0S(K  ,3) 
C         CHECK  FOR  PLANE  MOVEMENT 

IF  (((ABS(HSP0S{K+1 .1  )-XF)).LT.(ABS(HSP0S(K+1 ,1  )-HSP0S(K,1 )))) 
.AND.(ABS(HSP0S(K.1  )-XF).LT.MAXODX))  THEN 
HSP0S{K+1,1)=XF 
ENDIF 
IF  (((ABS(HSP0S{K+1 ,2)-YF)).LT.{ABS(HSP0S(K+1 ,2)-HSPOS(K,2)))) 
.AND.(ABS(HSP0S(K,2)-YF).LT.MAX0DY))  THEN 
HSP0S(K+1,2)=YF 
ENDIF 
IF  (((ABS(HSP0S(K+1 ,3)-ZF)).LT.(ABS(HSP0S(K+1 ,3)-HSPOS{K,3)))) 
.AND.(ABS(HSP0S(K,3)-ZF).LT.MAX0DZ))  THEN 
HSP0S(K+1 ,3)=ZF 
ENDIF 
C         CHECK  FOR  COMPLETION 

IF  ((ABS(HSP0S(K,1  )-XF).LT.MAX0DX).AND.(ABS(HSP0S(K,2)-YF).LT. 

MAX0DY).AND.(ABS(HSP0S(K,3)-ZF).LT.MAX0DZ))  THEN 
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GOT0 101 
ENDIF 
1 1  a>mNUE 

C         SAVE  DATA 
101  REWIND  02 

HSP0S(K+1.1)=XF 
HSP0S(K+1 ,2)=YF 
HSP0S(K+1 .3)=ZF 
00  243  1=  1,K+1 

WRITE(2.183)HSP0S(I.1),HSP0S(I,2),HSP0S(I.3) 
243  CX)NT1NUE 

1 83  FORMATC  •.T3,3(F1 0.8,2X)) 

RETURN 
END 
C 
C 

SUBROUTINE  FSTAR(XS,YS,ZS,XF,YF,ZF, 

MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
C         CALCULATES  UNEAR  FINISH  TO  NEXT  START 
DIMENSION  HSP0S(1 000,3) 
REAL  MAXODX,MAXODY,MAXODZ,VALMIN 
REWIND  02 

WRITE(9,*)  'FINISH  TO  START' 
WRITE(9,*)  XS,YS.ZS 
WRITE(9.*)  XF,YF,ZF 
0  WRITE(9,*)  J,K+1 

D0  769I=1,K+1 

READ  (02.*)  (HSP0S(I,L),L=1,3) 
769  CONTINUE 

DO12J=K+1,1000 
C         ALL  START  AND  FINISH  POINTS  THE  SAME 

IF  ((XS.EQ.XF).AND.{YS.EQ.YF).AND.(ZS.EQ.ZF))  THEN 

GOT0 102 
ENDIF 

DELX=MAXODX 
DELY=MAXODY 
DELZ=MAXODZ 
XFS=  ABS{XS-XF) 
YFS=ABS(YS-YF) 
ZFS=ABS(ZS-ZF) 
C 

IF  (ZFS.GE.XFS)  THEN 
GOTO  22 
ENDIF 
IF  (YFS.GE.XFS)  THEN 

DELX=DELX*(XFSA'FS) 
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DEI_Z=DE1_Z*(ZFSA'FS) 
ELSEIF  (XFS.GE.YFS)  THEN 

DELY=DELY'(YFS/XFS) 
DELZ=DELZ*(ZFS/XFS) 
ENDIF 
GOT032 
22  IF  (ZFS.GE.YFS)  THEN 

DELX=DELX*(XFS/ZFS) 
DELY=DELY*(YFS/ZFS) 
ELSEIF  (YFS.GEZFS)  THEN 

DELX=DELX*(XFSA'FS) 
DELZ=DELZ*{ZFSA'FS) 
ENDIF 
C         CHECK  FOR  NEGATIVE  DELX ,  DELY ,  OR  DELZ 
32  IF  ({XF-XS).LT.O)  THEN 

DELX=-ABS(DELX) 
ENDIF 
IF  ((YF-YS).LT.O)  THEN 

DELY=-ABS(DELY) 
ENDIF 
IF  ((ZF-ZS).LT.O)  THEN 

DELZ=-ABS(DELZ) 
ENDIF 
C         NEXT  POINT 

HSP0S(J+1,1)=HSP0S(J.1)+DELX 
HSP0S(J+1 ,2)=HSPOS(J,2)+DELY 
HSP0S(J+1 .3)=HSPOS(J.3)+DELZ 

WRITE(9,1 84)HSPOS{J+1 ,1  ),HSP0S(J+1 ,2),HSP0S(J+1 ,3) 
C         CHECK  FOR  PLANE  MOVEMENT 

IF(((ABS(HSP0S(J+1.1)-XF)).LT.(ABS(HSP0S(J+1.1)-HSP0S(J.1)))) 
.AND.(ABS(HSP0S(J,1  )-XF).LT.MAXODX))  THEN 
HSP0S(J+1,1)=XF 
ENDIF 
IF  (((ABS{HSP0S(J+1 ,2)-YF)).LT.{ABS(HSP0S(J+1 ,2)-HSPOS(J,2)))) 
.AND.(ABS(HSP0S(J.2)-YF).LT.MAX0DY))  THEN 
HSP0S{J+1,2)=YF 
ENDIF 
IF  (((ABS(HSP0S(J+1 .3)-ZF)).LT.(ABS(HSP0S(J+1 ,3)-HSPOS(J,3)))) 
.AND.(ABS(HSP0S(J,3)-ZF).LT.MAX0DZ))  THEN 
HSP0S(J+1.3)=ZF 
ENDIF 
C         CHECK  FOR  COMPLETION 

IF  ((ABS(HSP0S(J,1  )-XF).LE.ABS(DELX)).AND.{ABS(HSP0S(J,2)-YF) 

.LE.ABS(DELY)).AND.(ABS(HSP0S(J,3)-ZF).LE.ABS(DELZ)))  THEN 
GOT0 102 
ENDIF 
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12  CONTINUE 

102  HSP0S(J+1,1)=XF 

HSP0S(J+1 ,2)=YF 
HSP0S(J+1 .3)=ZF 
C         SAVE  DATA 
REWIND  02 
00  3431=  1,J+1 

WRITE(2.184)HSPOS(l,1),HSPOS(I,2),HSPOS(l,3) 
C  WRITE(9.184)  HSPOS(l.1).HSPOS(l.2),HSPOS(l.3) 

343  CONTINUE 

1 84  FORMATC  •,T3.6(F1 0.8,2X)) 

RETURN 
END 
C 

SUBROUTINE  FHOME(XS,YS,ZS,XF,YF.ZF, 
•    MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
C         CALC  UNEAR  FINISH  TO  HOME  MAXIMIZING  STEEPEST  PATH 
DIMENSION  HSP0S(1 000,3) 
REAL  MAXODX,MAXODY,MAXODZ,VALMIN 
REWIND  02 

WRITE(9,')  'FINISH  TO  HOME  ' 
D0  766I=1,K+1 

READ  (02.*)  (HSP0S(I,L).L=1,3) 
766  CONTINUE 

DO13J=K+1,1000 

DELX=MAXODX 
DELY=MAXODY 
DELZ=MAXODZ 
C         FINISH  POINT  IS  AT  ZERO 

IF  ((XF.LT.VALMIN).AND.(YF.LT.VALMIN).AND.(ZF.LT.VALMIN))  THEN 
GOT0 103 
ENDIF 
C 

IF  (ZF.GE.XF)  THEN 

GOTO  23 
ENDIF 
IF  (YF.GE,XF)  THEN 

DELX=DELX*(XFA'F) 
DELZ=DELZ*(ZFA'F) 
ELSE 

DELY=DELY*(YF/XF) 
DELZ=DELZ*(ZF/XF) 
ENDIF 
GOTO  33 
23  IF  (ZF.GE.YF)  THEN 

DELX=DELX*(XF/ZF) 
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DELY=DELY*(YF/ZF) 
ELSE 

DELX=DELX*(XFA'F) 
DELZ=DELZ'(ZFA'F) 
ENDIF 
33  HSP0S{J+1 .1  )=HSP0S(J,1  )-DELX 

HSP0S(J+1 ,2)=HSPOS(J,2)-DELY 
HSP0S(J+1 ,3)=HSPOS(J,3)-DELZ 

WRITE(9,1 85)  HSP0S(J+1 ,1  ),HSP0S(J+1 ,2).HSP0S(J+1 .3) 
C         CHECK  FOR  COMPLETION 

1F((HSP0S(J+1 ,1  ).LE.0.).AND.(HSPOS(J+1 .2).LE.0.).AND. 
(HSP0S(J+1 ,3).LE.O.))  THEN 
GOT0 103 
ENDIF 
0         CHECK  FOR  PLANE  MOVEMENT 

IF  ({HSP0S(J+1.1).LT.XF).AND.(HSP0S(J,1).GT.XF))  THEN 

HSP0S(J+1.1)=HSP0S(J,1) 
ENDIF 
IF  ((HSPOS(J+1,2).LT.YF).AND.(HSPOS(J,2).GT.YF))  THEN 

HSP0S(J+1 ,2)=HSPOS(J,2) 
ENDIF 
IF  ((HSPOS(J+1.3).LT.ZF).AND.(HSPOS(J,3).GT.ZF))  THEN 

HSP0S(J+1 ,3)=HSPOS(J,3) 
ENDIF 
13  CO^mNUE 

103  HSPOS(J+1,1)=0. 

HSP0S(J+1 ,2)=0. 
HSP0S(J+1 ,3)=0. 
C         SAVE  DATA 
REWIND  02 
DO  4431=  1,J+1 
WRITE(2.185)HSPOS(l,1),HSPOS(l,2),HSPOS{l.3) 
C    WRITE(9,185)  HSPOS(l,1),HSPOS(l,2).HSPOS(l.3) 
443    CONTINUE 

185    FORMAT{",T3,3(F10.8,2X)) 
RETURN 
END 
C         PARAMETRIC  SUBROUTINE  SFPAR 

SUBROUTINE  SFPAR(NJS.TF,DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN) 
DIMENSION  HSP0S{1 000,3) 

REALMAXODX,MAXODY,MAXODZ,VALMIN,DELT,TS,TF 
DT=DELT 
REWIND  02 
DO810l=1,J+1 

READ  (2,*)(HSPOS(l,L),L=1,3) 
810  CONTINUE 


T=TS 

K=J+1 

WHILE  (T.LT.TF)  DO 

820  IF(N.EQ.1)THEN 

DELX=PEQX1  (T+DT)-PEQX1  (T) 
ELSEIF(N.EQ.2)THEN 

DELX=PEQX2(T+DT)-PEQX2(T) 
ELSEIF  (N.EQ.3)  THEN 

DELX=PEQX3(T+DT)-PEQX3(T) 
ELSEIF  (N.EQ.4)  THEN 

DELX=PEQX4(T+DT)-PEQX4(T) 
ENDIF 

IF  (ABS(DELX).GT.MAXODX)  THEN 

DT=ABS{DT*MAXODX/DELX) 

GOTO  820 
ELSEIF  ((ABS(DELX).LE.VALMIN).OR.(DT.LE.VALMIN))THEN 

DELX=0.0 

DT=DELT 
ENDIF 
DTT=DT 
830  IF(N.EQ.1)THEN 

DELY=PEaY1  {T+DT)-PEQY1  (T) 
ELSEIF  (N.EQ.2)  THEN 

DELY=PEQY2(T+DT)-PEQY2(T) 
ELSEIF  (N.EQ.3)  THEN 

DELY=PEQY3(T+DT)-PEQY3(T) 
ELSEIF  (N.EQ.4)  THEN 

DELY=PEQY4(T+DT)-PEQY4(T) 
ENDIF 

IF  (ABS(DELY).GT.MAXODY)  THEN 

DT=ABS(DT*MAXODY/DELY) 

GOTO  830 
ELSEIF  ((ABS(DELY).LE.VALMIN).OR.(DT.LE.VALMIN))THEN 

DELY=0.0 

DT=DTT 
ENDIF 
DTT=DT 
840  IF(N.EQ.1)THEN 

DELZ=PEQZ1  (T+DT)-PEQZ1  (T) 
ELSEIF  (N.EQ.2)  THEN 

DELZ=PEQZ2(T+DT)-PEQZ2(T) 
ELSEIF  (N.EQ.3)  THEN 

DELZ=PEQZ3(T+DT)-PEQZ3(T) 
ELSEIF  (N.EQ.4)  THEN 

DELZ=PEQZ4(T+DT)-PEQZ4(T) 
ENDIF 
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IF  {ABS(DE1_Z).GT.MAX0DZ)  THEN 

DT=ABS(DrMAXODZ/DELZ) 
GOTO  840 
ELSEIF((ABS(DEL2).LE,VALMIN).0R.(DT.LE.VALMIN))THEN 
DELZ=0.0 
DT=DTT 
ENDIF 
C         MAXEDOUTYET 

IF  ((T+DT).GT.TF)  THEN 
DT=TF-T 
ENDIF 
C      CALCULATE  NEXT  POINT 
IF{N.EQ.1)THEN 

DELX=PEQX1  (T+DT)-PEQX1  (T) 
DELY=PEQY1  (T+DT)-PEQY1  (T) 
DELZ=PEQZ1  {T+DT)-PEQZ1  (T) 
ELSEIF  (N.EQ.2)  THEN 

DELX=PEQX2(T+DT)-PEQX2(T) 
DELY=PEQY2(T+DT)-PEQY2(T) 
DELZ=PEQZ2(T+DT)-PEQZ2(T) 
ELSEIF  (N.EQ.3)  THEN 

DELX=PEQX3(T+DT)-PEQX3(T) 
DELY=PEQY3(T+DT)-PEQY3(T) 
DELZ=PEQZ3(T+DT)-PEQZ3(T) 
ELSEIF  (N.EQ.4)  THEN 

DELX=PEQX4(T+DT)-PEQX4(T) 
DELY=PEQY4(T+DT)-PEQY4(T) 
DELZ=PEQZ4(T+DT)-PEQZ4(T) 
ENDIF 
C    WRITE  (9,1 1 5)  DT.DELX.DELY.DELZ 
1 1 5  FORMAT  ('  •,T3,4(F6.4,3X)) 

HSP0S(K+1 ,1  )=HSP0S(K,1  )+DELX 
HSP0S(K+1 ,2)=HSPOS(K,2)+DELY 
HSP0S{K+1 ,3)=HSPOS(K,3)+DELZ 

WRITE(9,183)HSPOS(K,1),HSPOS(K,2),HSPOS(K,3) 
T=T+DT 
K=K+1 
END  WHILE 
C         SAVE  RESULTS 
REWIND  02 
DO  2431=  1,K 

WRITE(2,183)HSPOS(M).HSPOS(l,2),HSPOS(l,3) 
C  WRITE(9,183)  HSPOS(M),HSPOS(l,2).HSPOS(l,3) 

243  CONTINUE 

K=K-1 
1 83  FORMATC  •,T3,3(F1 0.8,2X)) 
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RETURN 
END 
EOF: 
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E2     UNEAR  PATH1  MODIFICATIONS 

TOF: 

C  STB/EN  G.GOODWAY 

C  RPATH2-  UNEAR  CARTESIAN-  MODIFIED  FROM  VERSION 

C 

C         *  USER  MODIFICATION  AREA  * 

FUNCTION  EQUAT1(X,Y) 

EQUAT1=.500 

RERJRN 
END 
C         XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQATIY(X) 

EQAT1Y=1.25-X 

RETURN 
END 
C         EQUATION  2 

FUNCTION  EQUAT2(X,Y) 

EQUAT2=1.0 

RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT2 
FUNCTION  EQAT2Y(X) 

EQAT2Y=1.5-X 

RETURN 
END 
C        EQUATIONS 

FUNCTION  EQUAT3(X,Y) 

EQUAT3=1.0 

RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT3 
FUNCTION  EQAT3Y(X) 

EQAT3Y=1 .5-X 

RETURN 
END 
C        EQUATION  4 

FUNCTION  EQUAT4(X,Y) 

EQUAT4=1.0 

RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT4 
FUNCTION  EQAT4Y(X) 

EQAT4Y=1.5-X 
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RETURN 
END 
C         PARAMETRIC  X 

FUNCTION  PEQXfO 

PEQX=  .5*COS(T)+.5 
RETURN 
END 
C         PARAMETRIC  Y 

FUNCTION  PEQY(T) 

PEQY=  .5*SrN(T)+.5 
RETURN 
END 
C         PARAMETRIC  Z 

FUNCTION  PEQZ(T) 

PEQZ=  1/(12*3.141529) 
RETURN 
END 
C         END  OF  FUNCTION  DESCRIPTION  AREA 
C 

C         *  EQUATION  BOUNDS  AREA   * 

C         FIRST  PATH  EQUATION 
C         START/FINISH  POINTS 

XS1=1. 

XF1=0.25 
C         CALCULATE  Y  START/FINISH  POINTS 

YS1=EQAT1Y(XS1) 

YF1=EQAT1Y(XF1) 
C         CALCULATE  Z  START/FINISH  POINTS 

ZS1=EQUAT1(XS1,YS1) 

ZF1=EQUAT1(XF1,YF1) 
C         OUTPUT  START  AND  FINISH  POINTS  FOR  EQUATION  1 

WRITE  (9,65)  'I'.XSI  ,YS1  ,ZS1  ,XF1  ,YF1  .ZF1 

WRITE  (6,65)  'r.XSI  ,YS1  ,ZS1  ,XF1  ,YF1  ,ZF1 
C         SECOND  PATH  EQUATION 
C         START/FINISH  POINTS 

XS2=0.5 

XF2=1 .0 
C         CALCULATE  Y  START/FINISH  POINTS 

YS2=EQAT2Y(XS2) 

YF2=EQAT2Y(XF2) 
C         CALCULATE  Z  START/FINISH  POINTS 

ZS2=EQUAT2(XS2,YS2) 

ZF2=EQUAT2(XF2,YF2) 
C         OUTPUT  START  AND  FINISH  POINTS  FOR  EQUATION  2 
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WRITE  (9,65)  •2',XS2,YS2,ZS2,XF2,YF2,ZF2 
WRITE  (6,65)  '2',XS2,YS2,ZS2,XF2,YF2,ZF2 
C 

C*  CALCULATE  PATH  MOVEMENT  * 

C         PATH  FORMATION 

XS=XS1 

YS=YS1 

ZS=ZS1 

XF=XF1 

YF=YF1 

ZF=ZF1 
0         HOME  TO  START 

CALLHSTAR(XS,YS.ZS,XF.YF,ZF.MAXODX,MAXODY.MAXODZ,VALMIN,J,K) 

0 

Q         ....................... 

0         *  START  TO  FINISH  EQUAT1  * 
^         *********************** 

N=1 

CALLSTARF(XS,YS,ZS.XF,YF,ZF,N,MAXODX,MAXODY,MAXODZ,VALMIN 
.J.K) 
0         LINEAR  FINISH  EQUAT1  TO  START  EQUAT2-FAST 

XS=XF1 

YS=YF1 

ZS=ZF1 

XF=XS2 

YF=YS2 

ZF=ZS2 

CALLFSTAR(XS,YS.ZS,XF,YF,ZF,MAXODX,MAXODY.MAXODZ,VALMIN,J,K) 
0         START  TO  FINISH  EQUATION  2 
XS=XS2 

YS=YS2 

ZS=ZS2 

XF=XF2 

YF=YF2 

ZF=ZF2 

N=2 

CALLSTARF(XS,YS,ZS,XF.YF.ZF,N,MAXODX,MAXODY,MAXODZ.VALMIN 
.J.K) 
C         FINISH  EQUAT2  TO  HOME 

XF=XF2 

YF=YF2 

ZF=ZF2 

CALLFHOME(XS,YS,ZS,XF,YF,ZF,MAXODX.MAXODY,MAXODZ,VALMIN,J,K) 
0 
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Q      ........ 

C         •  FINISH  POINT  FORMATION  * 
Q         ... ... 

END  MODIFICATION  REGION 


E.3      LINEAR  PATH  2  PROGRAM 
DESCRIBED  IN  SECTION  E.1 
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E4     CIRCULAR  PATH  MOTION 

TOR 

C         STEVEN  G.GOODWAY 

C         R0BPATH4  FORTRAN 

C 

C         *  USER  MODIFICATION  AREA  * 
^         **************************** 

FUNCTION  EQUAT1(X,Y) 
EQUAT1=0.5 
RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQAT1Y(X) 

EQAT1Y=(SQRT(ABS(.25-{X-.5)**2)))+.5 
RETURN 
END 

FUNCTION  EQUAT2(X,Y) 
EQUAT2=0.5 
RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT2 
FUNCTION  EQAT2Y{X) 

EQAT2Y=-(SQRT(ABS(.25-(X-.5)"2)))+.5 
RETURN 
END 
C         END  OF  FUNCTION  DESCRIPTION  AREA 
C 

C         MAIN  PROGRAM  AREA 
C         USER  PARAMETER  ADJUSTMENTS 

C 

/^         *********************** 

C         *  EQUATION  BOUNDS  AREA   * 

r\  *********************** 

C         FIRST  PATH  EQUATION 
C         START/FINISH  POINTS 

XS1=1. 

YS1=0.5 

XF1=0.0 

YF1=0.5 
C         CALCULATE  Z  START/FINISH  POINTS 

ZS1=EQUAT1(XS1,YS1) 

ZF1=EQUAT1(XF1,YF1) 
C 
C  SECOND  PATH  EQUATION 
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c 
c 
c 
c 
c 


START/FINISH  POINTS 

XS2=0.0 

YS2=0.5 

XF2=1 .0 

YF2=0.5 

CALCULATE  Z  START/FINISH  POINTS 

ZS2=EQUAT2(XS2.YS2) 

ZF2=EQUAT2(XF2.YF2) 


*  CALCULATE  PATH  MOVEMENT  * 

PATH  FORMATION 

XS=XS1 

YS=YS1 

ZS=ZS1 

XF=XF1 

YF=YF1 

ZF=ZF1 

HOME  TO  START 

CALLHSTAR(XS.YS.ZS.XF.YF.ZF.MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 

*  START  TO  FINISH  EQUAT1  * 

A-********************** 

c 
c 
c 
c 

ACC=0.01 
C         ADJUST  FOR  SLOW  SPEED 

MAXODX=MAXODX'ACC 

MAXODY=MAXODY/ACC 

MAXODZ=MAXODZ/ACC 

N=1 
CALLSTARF(XS,YS^S,XF,YF,ZF,N,MAXODX,MAXODY.MAXODZ,VALMIN 
*    .J.K) 
C 
C         LINEAR  FINISH  EQUAT1  TO  START  EQUAT2-FAST 

MAXODX=ACC*MAXODX 

MAXODY=ACC*MAXODY 

MAXODZ=ACC*MAXODZ 

XS=XF1 

YS=YF1 

ZS=ZF1 

XF=XS2 

YF=YS2 

ZF=ZS2 

CALLFSTAR(XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
C         START  TO  FINISH  EQUATION  2 
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MAXODX=MAXODX/ACX) 

MAXODY=MMODY/ACC 

MAX0DZ=MAX0D2/ACC 

XS=XS2 

YS=YS2 

ZS=ZS2 

XF-XF2 

YF=YF2 

ZF=ZF2 

N=2 
CALLSTARF(XS,YS^S,XF,YF.ZF,N.MAXODX,MAXODY,MAXODZ,VALMIN 
*    ,J.K) 
C 
C         ADJUST  FOR  FAST  MOVEMENT 

MAXODX=ACC*MAXODX 

MAXODY=ACC*MAXODY 

MAXODZ-ACC*MAXODZ 
C         FINISH  EQUAT3  TO  HOME 

XF=XF2 

YF=YF2 

ZF=ZF2 

CALLFHOME(XF,YF,ZF.MAXODX,MAXODY,MAXODZ.VALMIN,J,K) 


********************** 


C         *  FINISH  POINT  FORMATION  ' 
C 
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E.5  HELIX  PATH  1  AND  2 

TCF: 

C         STEVEN  a  GOODWAY 

C         R0BPATH5  FORTRAN-PARAMETRIC  SUBROUTINE  ADDED 

C         NPS/WEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 
Q         * 

C         *  USER  MODIFICATION  AREA  * 

Q 

FUNCTION  EQUAT1(X,Y) 
EQUAT1=0.5 
RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQAT1Y(X) 

EQAT1Y=(SQRT(ABS(.25-(X-.5)"2)))+.5 
RETURN 
END 

FUNCTION  EQUAT2(X,Y) 
C        EQUAT2=((ABS(.25-(X-.5)"2  -{Y-.5)"2  ))**.5+.5) 
EQUAT2=0.5 
RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT2 
FUNCTION  EQAT2Y(X) 

EQAT2Y=-{SQRT(ABS(.25-(X-.5)"2)))+.5 
RETURN 
END 
C         PARAMETRIC  X 

FUNCTION  PEQX(T) 

PEQX=  .5*COS(T)+.5 
RETURN 
END 
C         PARAMETRIC  Y 

FUNCTION  PEQYfO 

PEQY=  .5*SIN(T)+.5 
RETURN 
END 
C         PARAMETRIC  Z 

FUNCTION  PEQZ(T) 
C         HELIX  3-CYCLE 
C  PEQZ=T/(6*3.141529) 

C         HELIX  6-CYCLE 

PEQZ=T/(1 2*3.1 41 529) 
RETURN 
END 
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C         END  OF  FUNCTION  DESCRIPTION  AREA 

C 

C         MAIN  PROGRAM  AREA 

C         USER  PARAMETER  ADJUSTMENTS 

ACC=1.0 
C         MODIFY  DELT  FOR  PARAMETRIC 
DELT=0.1 

C         "EQUATION  BOUNDS  AREA   * 

C         FIRST  PATH  EQUATION 

C         PARAMETRIC  START  AND  FINISH  POINTS 

TS=0.0 
C         HELIX  3-CYCLE 
C  TF=6*PI 

C         HELIX  6-CYCLE 

TF=12*PI 

XS1=PEQX(TS) 

YS1=PEQY(TS) 

ZS1=PEQZ(TS) 

XF1=PEQX(TF) 

YF1=PEQY(TF) 

ZF1=PEQZ(TF) 
C 

C         *  CALCULATE  PATH  MOVEMENT  * 

C         PATH  FORMATION 

XS=XS1 

YS-YS1 

ZS=ZS1 

XF=XF1 

YF=YF1 

ZF=ZF1 
C         HOME  TO  START 

CALLHSTAR(XS,YS,ZS,XF.YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 

C         *  START  TO  FINISH  EQUAT1  * 

C         START  TO  FINISH  PARAMETRIC  EQUATION 

CALLSFPAR(TS,TF.DELT,MAXODX,MAXODY,MAXODZ,J,K.VALMIN) 
C         FINISH  EQUAT3  TO  HOME 
XF=XF1 
YF=YF1 
ZF=ZF1 
CALLFHOMEp(F.YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 
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••••*•*••••*****••••*•• 


C         •  FINISH  POINT  FORMATION  * 

C 

END  HEUX  PATH  1  &  2  MODIFICATION 
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E.6  UNEAR  PATH  MODIFICATIONS 

TOF: 

C         STEVEN  a  GOOD  WAY 

C         R0BPATH6  FORTRAN-PARAMETRIC  SUBROUTINE  ADDED 

C         UNEAR  PARAMETRIC/CATRSIAN 

C         *  USER  MODIFICATION  AREA  * 

C         PARAMETRIC  X  --EQUATION  1 

FUNCTION  PEQX1(T) 
PEQX1=1. 

RETURN 

END 
C         PARAMETRIC  Y  -EQUATION  1 

FUNCTION  PEQY1(T) 
PEQY1=.25+T 

RETURN 

END 
C         PARAMETRIC  Z  -EQUATION  1 

FUNCTION  PEQZ1(T) 
PEQZ1=.5 

RETURN 

END 
C         PARAMETRIC  X  -EQUATION  2 

FUNCTION  PEQX2(T) 
PEQX2=  .75-T 

RERJRN 

END 
C         PARAMETRIC  Y  -EQUATION  2 

FUNCTION  PEQY2(T) 
PEQY2=1. 

RETURN 

END 
C         PARAMETRIC  Z  -EQUATION  2 

FUNCTION  PEQZ2(T) 
PEQZ2=  .5 

RETURN 

END 
C         PARAMETRIC  X  -EQUATION  3 

FUNCTION  PEQX3(T) 
PEQX3=  0.0 

RETURN 

END 
C         PARAMETRIC  Y  -EQUATION  3 

FUNCTION  PEQY3(T) 
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PEQY3=1. 
RETURN 
END 
C         PARAMETRIC  Z  --EQUATION  3 
FUNCTION  PEQZ3(T) 
PEQZ3=  2.*T 
RETURN 
END 
C         CARTESIAN  -EQUATION  4 
FUNCTION  EQUAT4(X,Y) 
EQUAT4=Y 
RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT4 
FUNCTION  EQAT4Y(X) 
EQAT4Y=1 .  -  X 
RETURN 
END 
C         END  OF  FUNCTION  DESCRIPTION  AREA 
C         MAIN  PROGRAM  AREA 
C         USER  PARAMETER  ADJUSTMENTS 

C         'EQUATION  BOUNDS  AREA   * 

C 

C         PARAMETRIC  START  AND  FINISH  POINTS 
TS=0.0 
TF=0.5 
C         PARAMETRIC  EQUATION  1  START/FINISH 

XS1=PEQX1(TS) 

YS1=PEQY1(TS) 

ZS1=PEQZ1(TS) 

XF1=PEQX1(TF) 

YF1=PEQY1(TF) 

ZF1=PEQZ1(TF) 
C         PARAMETRIC  EQUATION  2  START/FINISH 

XS2=PEQX2(TS) 

YS2=PEQY2(TS) 

ZS2=PEQZ2(TS) 

XF2=PEQX2(TF) 

YF2=PEQY2(TF) 

ZF2=PEQZ2(TF) 
C         PARAMETRIC  EQUATION  3  START/FINISH 

XS3=PEQX3(TS) 

YS3=PEQY3(TS) 

ZS3=PEQZ3(TS) 
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XF3=PEQX3(TF) 

YF3=PEQY3(TF) 

ZF3=PEQZ3(TF) 
C         CARTESIAN  EQUATION  4  START/FINISH 

XS4-0J5 

XF4=0.25 
C         CALCULATE  Y  START/FINISH  POINTS 

YS4=EQAT4Y(XS4) 

YF4=EQAT4Y(XF4) 
C         CALCULATE  Z  START/FINISH  POINTS 

ZS4=EQUAT4(XS4,YS4) 

ZF4=EQUAT4(XF4,YF4) 
0 
C 
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C         *  CALCULATE  PATH  MOVEMENT  * 
Q         • *.«. 

C 

C         PATH  FORMATION 

XS=XS1 

YS=YS1 

ZS=ZS1 

XF=XF1 

YF=YF1 

ZF=ZF1 
C         HOME  TO  START 

CALLHSTAR(XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 

C         *  START  TO  FINISH  EQUAT1  * 

C         START  TO  FINISH  PARAMETRIC  EQUATION 

N=1 

CALLSFPAR(TS,TF,DELT,MAXODX,MAXODY.MAXODZ,J,K,VALMIN,N) 
C         UNEAR  FINISH  EQUAT1  TO  START  EQUAT2 

XS=XF1 

YS=YF1 

ZS=ZF1 

XF=XS2 

YF=YS2 

ZF=ZS2 

WRITE  (9,*)  XS,YS,ZS.XF,YF,ZF 

CALLFSTAR(XS,YS,ZS,XF,YF,ZF,MAX0DX,MAX0DY,MAX0DZ,VAIJV1IN,J,K) 
C         START  TO  FINISH  EQUATION  2 

XS=XS2 

YS=YS2 

ZS=ZS2 

XF=XF2 

YF=YF2 

ZF=ZF2 

N=2 

CALLSFPAR(TS,TF,DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN,N) 
C        LINEAR  FINISH  EQUAT2  TO  START  EQUAT3 

XS=XF2 

YS=YF2 

ZS=ZF2 

XF=XS3 

YF=YS3 

ZF=ZS3 

CALLFSTAR(XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ.VALMIN,J,K) 
C         START  TO  FINISH  EQUATION  3 
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XS=XS3 

YS=YS3 

ZS=ZS3 

XF=XF3 

YF-YF3 

ZF-ZF3 

N=3 

CALLSFPAR(TS,TF.DELT,MAXODX,MAXODY,MAXODZ,J,K,VALMIN.N) 

LINEAR  FINISH  EQUAT2  TO  START  EQUAT3 
XS=XF3 
YS=YF3 
ZS-ZF3 
XF=XS4 
YF=YS4 
ZF=ZS4 
CALLFSTAR(XS.YS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 

START  TO  FINISH  EQUATION  4 
XS-XS4 
YS=YS4 
ZS=ZS4 
XF=XF4 
YF=YF4 
ZF=ZF4 
N=4 
CALL  STARF(XS,YS.ZS,XF,YF,ZF,N, 

MAXODX.MAXODY,MAXODZ.VALMIN,J,K) 

FINISH  EQUAT4  TO  HOME 
XF=XF4 
YF=YF4 
ZF=ZF4 
CALLFHOME(XF,YF,ZF,MAXODX,MAXODY.MAXODZ,VALMIN,J,K) 


*******«**«****«***««4* 


C         •  FINISH  POINT  FORMATION  * 


288 


E.7  SINUSOIDAL  PATH  MODIFICATIONS 

TOF: 

C         STEVEN  G.GOODWAY 

C         R0BPATH7  FORTRAN-PARAMETRIC  SUBROUTINE  ADDED 

C         *  USER  MODIFICATION  AREA  * 

FUNCTION  EQUAT1{X.Y) 

EQUAT1  =ABS(COS(9*3.1 41 529*X)) 
RETURN 
END 
C         XY  PROJECTION  FOR  EQUAT1 
FUNCTION  EQAT1Y(X) 

EQAT1  Y=(ABS(SIN(6*3.1 41 529*X))) 
RETURN 
END 
C         END  OF  FUNCTION  DESCRIPTION  AREA 
C 

C         MAIN  PROGRAM  AREA 
C         USER  PARAMETER  ADJUSTMENTS 

C         'EQUATION  BOUNDS  AREA   * 

c 

C         FIRST  PATH  EQUATION 
C         START/FINISH  POINTS 

XS1=1. 

XF1=0.0 
C         CALCULATE  Y  START/FINISH  POINTS 

YS1=EQAT1Y{XS1) 

YF1=EQAT1Y(XF1) 
C         CALCULATE  Z  START/FINISH  POINTS 

ZS1=EQUAT1(XS1,YS1) 

ZF1=EQUAT1(XF1,YF1) 

C         *  CALCULATE  PATH  MOVEMENT  * 

c 

C         PATH  FORMATION 
XS=XS1 
YS=YS1 
ZS=ZS1 

XF=XF1 
YF=YF1 
ZF=ZF1 
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C         HOME  TO  START 

CALLHSTAR(XS,YS,ZS,XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN,J,K) 

C         *  START  TO  FINISH  EQUAT1  * 

N=1 

CALLSTARF(XS,YS^S,XF,YF^F,N,MAXODX,MAXODY,MAXODZ,VALMIN 
.J.K) 
C 
C         FINISH  EQUAT3  TO  HOME 

CALLFHOME(XF,YF,ZF,MAXODX,MAXODY,MAXODZ,VALMIN.J,K) 
C 

C         *  FINISH  POINT  FORMATION  * 

Q  .... ............. 
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APPENDIX  F 
3-DIMENSIONAL  PLOTTING  PROGRAM 

This  appendix  contains  the  3-Dimensional  plotting  program.  In  the 
current  listing  the  plane  projections  will  not  be  drawn.  For  plane 
projections  just  enable  all  three  CURVE  functions.  A  reminder  that  large 
data  points  will  require  a  lot  of  memory,  upwards  to  40%  of  user  A-disk, 
and  that  parts  of  the  axis  may  be  ignored  by  the  DISSPLA  routine.  Format  of 
the  data  points  are  a  integer  J,  which  represents  one  less  then  the  total  of 
the  three  tuples  in  table  form,  which  follow. 


TOF: 

C  STEVEN  a  GOODWAY 

C  R0BCX)MP1  .FORTRAN 

C  NPS/WEAPONS  ENGINEERING/ELECTRICAL  ENGINEERING 

C  CARTESIAN  ROBOT  SIMULATION  {R0BSIM1  .FORTRAN} 

C  THIS  PROGRAM  TAKES  THE  THREE  TUPLE  OF  POINTS  (X,Y^ 

C  THAT  DESCRIBES  THE  PATH  THAT  THE  ENDPOINT  OF  A  CARTESIAN 

C  ROBOT  IS  TO  FOLLOW  AND  PLOTS  THEM  IN  THREE  DIMENSIONS. 

C 

C 

C  MAIN  PROGRAM  AREA 

C  USER  PARAMETER  ADJUSTMENTS 

DIMENSION  P0S(1 000,3).X(1 000),Y(1 000).Z(1 000) 
C 
C 

C  *  3-D  PLOT  SUBROUTINE  * 

C 

INTEGER  N.l 

CALL  PAGE(11., 8.5) 
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C  GET  DATA 

REWIND  02 

READ  (2,*)  J 
DO  90  1=1  ,J-1 

READ(2,'){POS{l,L),L=1,3) 
X(I)=P0S(I,1) 
Y(I)=P0S(I,2) 
Z(I)=P0S(I,3) 
90        CXDNTINUE 
C  OUTPUT  TO  TEK  61 8 

CALLTEK618 

CALLNOBRDR 
C  SET  UP  Z-AXIS  PARALLEL  TO  X-Y  PLANE 

CALL  ZAXANG(-90) 
C  SET  UP  TITLE  AND  PLOT  AREA 

CALLAREA2D(7.,7.) 

CALL  HEADINC3D  PATH  TRAJECTORY  $M00,2.,2) 

CALL  HEADINCROBOT  PATH  MOTIONS  $',100,1. 5,2) 
C  SET  UP  AXIS  LABELS  AND  DEFINES  3D  WORKSPACE 

CALL  V0LM3D(  7.,7.,7.) 

CALL  X3NAMECX-POS',5) 

CALL  Y3NAME('Y-POS',5) 

CALL  Z3NAME('Z"POS',5) 
C  DEFINE  VIEWPOINT 

0  CALL  VUABS(-30.,-1 0..23.) 

0  DEFINE  THE  AXIS  SYSTEM 

CALL  GRAF3D(0.0,'SCALE,1 .0,0.0,'SCALEM  .0,0.0, 
•SCALE.1 .0) 
C  3-D  CURVE  PLOTTING 

CALLP0LY3 

CALLCURV3D(X,Y,Z,J.O) 
0  PUT  2-D  PLOT  ON  FLOOR  OF  3-D 

CALL  GRFITI(0.,0.,0.,1  .,0.,0.,0.,1  .,0.) 

CALL  AREA2D(7.,7.) 

CALL  GRAF(0.0,'SCALEM  .0,0.0,'SCALE,1 .0) 
C  CALL  CURVE(X,Y,J,0) 

C  PUT  2-D  PLOT  OF  XZ  DATA  ON  BACK  WALL  OF  3-D 

CALL  GRFITI(0.,7.,0.,1  .,7.,0.,1  .,7.,2.) 

CALL  XNAME('X-P0S',5) 

CALLAREA2D(7.,7.) 

CALL  GRAF(0.0,'SCALE,1 .0,0.0,'SCALE',1 .0) 
0  CALLCURVE(X,Z.J.O) 
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C  PUT  2D  PLOT  OF  YZ  DATA  ON  OTHER  BACK  WALL 

CALLGRFITI(7.,7.,0..7.,5.,0..7..5..5.) 

CALL  XNAMECY-POS',5) 

CALLYNAMECZ-POS',5) 

CALL  AREA2D(7.,7.) 

CALL  GRAF{0.0,'SCALEM  .O.O.O.'SCALEM  .0) 
C  CALLCURVE(Y,Z,J,0) 

C  TERMINATE  PLOT 

CALL  ENDPL(O) 
CALL  DONEPL 
STOP 

END 
EOF: 
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APPENDIX  G 
COORDINATE  TRANSFORMATION  PROGRAM 

This  appendix  contains  the  coordinate  transformation  programs 
necessary  to  convert  from  Cartesian  Coordinates  to  Articulated  Coordinates 
and  back  to  Cartesian  Coordinates,  with  intermediate  transformation 
through  spherical  coordinate  system. 

TOF: 

C  STEVEN  GOODWAY 

C  WEAPONS  SYSTEMS  ENGINEERING/ELECTRICAL  ENGINEERING 

C  TRANSFORM  FORTRAN 

C  THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  PERFORM  COORDINATE 

C  TRANSFORMATIONS  AND  TO  PERFORM  MISCILANEOUS  FUNCTIONS 

0  SUCH  AS  SCAUNG  AND  UMITING  FUNCTIONS.  THE  COORDINATES 

0  TRANSFORMATIONS  ARE: 
0 

C  FROM:                    TO:                            SUBROUTINE: 

0  CARTESIAN             SPHERICAL               CARSP 

0  SPHERICAL            CARTESIAN                SPCAR 

C  SPHERICAL            ARTICULATED            SPART 

C  ARTICULATED         SPHERICAL               ARTSP 
0 

0  THE  MISCELLANEOUS  SPECIAL  FUNCTIONS  ARE: 

0  CARTESIAN  SCAUNG                                [CARSC] 

0  SPHERICAL  SCAUNG                                [SPHSC] 

0  CARTESIAN  LIMITING                              [CARLI] 

C  SPHERICAL  UMITING                              [SPHLI] 
C 

C    TO  OPERATE  THE  PROGRAM  A  PATH  OF  SUBROUTINES  MUST  BE  GENERATED 

C  THAT  IS  IN  THE  ORDER  THAT  THE  USER  INTENDS  TO  PERFORM  IN. 

0  ALSO  THE  USER  MUST  DECLARE  SEVERAL  FILEDEFS  SO  THAT  THE  FLOW 

0  IS  PROPER. 
0 

C  THE  FILEDEFS  FOR  CARTESIAN  TO  ARTICULATED  ARE  PRIOR  TO 

0  SIMULATION: 

0  UNIT          SAMPLE                     TiPE 

0  02              PATH4                        DATA 

0  03              SC0MP4                     DATA 

0  04              C0MP4                       DATA 

294 


C  05  SPATH4  DATA 

C  09  TERM 

C  14  APATH4  DATA 

C  15  ASPATH4  DATA 

C  16  SCPATH4  DATA 

C 

C         THE  FILEDEFS  FOR  ARTICULATED  TO  CARTESIAN  AFTER  SIMULATION  ARE: 

0  UNIT  SAMPLE  TYPE 

0  09  TERM 

0  14  DAPATH4  DATA 

0  16  DCPATH4  DATA 

0 

Q  •*«*•*••*••••..*..«.*.*..* 

0        *  MAIN  USER  PATH  FORMATION  * 

C 

0         CARTESIAN  TO  SPHERICAL 

REAL  MAX 
MAX=1.0 
SMAX=0.5 
SF=1.0 
0  CALLCARU 

0  CALL  CARSC(SF,MAX) 

CC  CALLCARU 

CC  CALLCARSP 

CCCC  SPHERICAL  TO  ARTICULATED 
CC  SF=1.0 

C  CALLSPHLI 

C  CALL  SPHSC(SF,SMAX) 

CC  CALLSPHLI 

CC  CALLSPART 

CCCC  ARTICULATED  TO  SPHERICAL 

CALLARTSP 
OOOC  SPHERICAL  TO  CARTESIAN 
CC  SF=1/SMAX 

C  CALLUSPSC(SF) 

CALLSPCAR 
STOP 
END 
C        ""•"*•""♦ 

C         •  SUBROUTINES  * 
C        •""" 

c 

pk  A**************** 

C        *  CARSP  SUBRUTINE  * 
C        """ — """* 
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c 

C         THIS  PROGRAM  TRANSFORMS  FROM  CARTESIAN  TO  SPHEREICAL 
C         COORDINATE  SYTEMS. 
0        POS(l,L)  =  PATH  POSITION  IN  CARTESIAN 
0        SPPOS(l,L)  =  PATH  POSITION  IN  SPHERICAL 
C         HOME  FOR  THE  CARTESIAN  ROBOT  IS  (0.0,0.0,0.0)  HOWEVER 
0         HOME  FOR  THE  ARTICULATED  ROBOT(SPHERICAL)  IS  (0.5,0.5,0.5). 
0         IF  INTENDING  TO  TRANSFORM  INSURE  THAT  THE  ORIGINAL  PATH 
0         STARTS  AT  THE  SPHERICAL  HOME  POSITION. 
SUBROLTTINECARSP 
INTEGER  l,J,K,L 

DIMENSION  P0S(1 000,3),SPPOS(1 000,3) 
REWIND  02 
0         INPUT  CARTESIAN  PATH 
READ(2,*)  J 
DO300l=1,J-1 
READ(2,*)(POS(l,L),L=1,3) 
300  CONTINUE 

0         CALCULATIONS  FROM  CARTESIAN  TO  SPHERICAL 

DO400M.J-1 
0         CALJCRHO 

SPP0S(I,1  )=(SQRT((P0S(I,1  )-0.5)"2+(POS(l,2)-0.5)"2+( 
POS(I,3)-0.5)"2)) 
0         CALC  THETA  FROM  XY  PROJECTION 

B=(SQRT((POS(l.1)-.5)"2+(POS(l,2)-.5)**2)) 
A=(SQRT((POS(l,1)-1.)**2+(POS(l,2)-.5)"2)) 
C=.5 

Pl=3.1 41 5926536 
IF  (B.EQ.0.0)  THEN 

SPPOS(I,2)=0.0 
ELSE 

SPPOS(l,2)=ACOS((B**2+C"2-A**2)/(2.*C*B)) 
IF  (POS(I,2).LT.0.5)  THEN 

SPP0S(I,2)=  2  *  PI  -SPP0S(I,2) 
ENDIF 
ENDIF 
C         CALC  PHI  IN  THREE  DIMENSIONS 

F=(SQRT(B"2+(POS(l,3)-1  .)"2)) 
R=SPP0S(I,1) 
IF  (R.EQ.0.0)  THEN 

SPPOS(I,3)=0.0 
ELSE 

SPPOS(l,3)=(ACOS((C**2+R"2-F**2)/(2.*C*R))) 
ENDIF 
400  CONTINUE 

0         PRINT  AND  I/O  STATEMENTS 
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WRITE(9.*)  j;    CARTESIAN  TO  SPHERICAL 

WRITE(5,*)  J 
DO500l=1,J-1 
WRITE  (5.*)(SPPOS(I.L),L=1,3) 

500  Ca^NUE 

742  FORMAT  ('  •,T2,I4,2X,6(F5.3,2X)) 

RETURN 
END 
0 

0      ****** — ******* 

0        *  SPART  SUBROUTINE  * 

0       ***** ***** 

0 

0         THIS  PROGRAM  TRANSFORMS  FROM  SPHERICAL  TO  ARTICULATED 
0         COORDINATE  SYTEMS.  THE  SPHERICAL  COORDINATE  SYSTEM  IS  COMPOSED 
0         THE  THREE  TUPLE  (RHO.THETA.PHI)  AND  THE  ARTICULATED  SYSTEM 
0         IS  COMPOSED  OF  THE  THREE  TUPLE  (CP1  ,CP2,CP3)  WHICH  DISCRBES 
C         THE  COMANDED  ANGULAR  POSITION  FOR  THE  THREE  ROBOT  MOTIONS  AND 
0         EXPRESSED  IN  RADIANS.  TO  PROPERLY  WORK  THE  SPHERICAL  SYSTEM 
C  MUST  EXIST  WITHIN  A  1  UNIT  DIAMETER  SPHERE. 

SUBROUTINE  SPART 

INTEGER  LJ.K.L 

DIMENSION  ARTP0S(1 000,3).SPPOS(1 000,3) 
0         INPUT  CARTESIAN  PATH 

REWINDS 

READ(5,*)  J 

DO  301  l=1,J-1 

READ(5,*)(SPP0S{I.L).L=1 ,3) 
301  COm-|NUE 

0         CALCULATIONS  FROM  CARTESIAN  TO  SPHERICAL 

Pl=3.1 41 5926536 

DO  401  l=1,J-1 
0         CALCULATE  CP1 

ARTP0S(I,1)=SPP0S(I,2) 
0         CALCULATE  CP2 

ARTPOS(l.2)=PI-SPPOS(l,3)+ACOS(2*SPPOS(M)) 
C         CALCULATE  CP3 

ARTPOS(l.3)=PI-2*(ACOS(2*SPPOS{l,1))) 
401  CONTINUE 

C         PRINT  AND  I/O 

REWIND  14 

WRITE(9,*)  'SPHERICAL  TO  ARTICULATED  (R,T,P,CP1,CP2,CP3)' 

WRITE(14,')J 

DO  501  1=1  ,J-1 

WRITE  (1 4,*)(ARTP0S{I,L),L=1 ,3) 

501  CONTINUE 
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742  FORMAT  ('  '.T2.I4,2X,6(F5.3.2X)) 

RETURN 
END 


«««*«««««*«*«««««*«« 


*  ARTSP  SUBROUTINE 


C 

C         THIS  PROGRAM  TRANSFORMS  FROM  ARTICULATED  TO  SPHERICAL 
C         COORDINATE  SYTEMS.  THE  SPHERICAL  COORDINATE  SYSTEM  IS  COMPOSED 
0         THE  THREE  TUPLE  (RHO.THETA.PHI)  AND  THE  ARTICULATED  SYSTEM 
0         IS  COMPOSED  OF  THE  THREE  TUPLE  (CP1  ,CP2.CP3)  WHICH  DESCRIBES 
0         THE  COMANDED  ANGUU^  POSITION  FOR  THE  THREE  ROBOT  MOTIONS  AND 
C         EXPRESSED  IN  RADIANS.  TO  PROPERLY  WORK  THE  SPHERICAL  SYSTEM 
C         MUST  EXIST  WITHIN  A  1  UNIT  DIAMETER  SPHERE. 

SUBROUTINE  ARTSP 

INTEGER  IJ.K.L 

DIMENSION  ARTP0S(1 000,3),SPPOS{1 000,3) 
C         INPUT  CARTESIAN  PATH 

REWIND  14 

READ{14,*)  J 

DO  302  1=1  ,J-1 

READ(14,*)(ARTPOS{I.L).L=1,3) 
302  CO^^■|NUE 

C         CALCULATIONS  FROM  ARTICULATED  TO  SPHERICAL 

Pl=3.1 41 5926536 

DO  402  1=1  ,J-1 
0         CALCULATE  RHO 

SPPOS(l,1)=.5*COS({PI-ARTPOS(l,3))/2) 
0         CALCULATE  THETA 

SPP0S(!,2)=ARTP0S(I,1) 
C         CALCULATE  PHI 

SPPOS(l,3)=PI-ARTPOS(l.2)+(ACOS(2*SPPOS(l.1))) 
402  CONTINUE 

0         PRINT  AND  I/O 

REWIND  15 

WRITEO,*)  'ARTICULATED  TO  SPHERICAL  (CP1  ,CP2,CP3,R.T,P)' 

WRITE(15,*)J 

DO  502  1=1  .J-1 

WRITE  (15.*){SPPOS(I.L),L=1.3) 
502  CONTINUE 

742  FORMAT  ('  •,T2,I4,2X,6(F5.3,2X)) 

RETURN 
END 
C 
0 
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C         •  SPCAR  SUBROUTINE  * 

C 

C         THIS  PROGRAM  TRANSFORMS  FROM  SPHERICAL  TO  CARTESIAN 

C         COORDINATE  SYTEMS.  THE  SPHERICAL  COORDINATE  SYSTEM  IS  COMPOSED 

0         THE  THREE  TUPLE  (RHO.THETA,PHI)  AND  THE  CARTESIAN  SYSTEM 

C         IS  COMPOSED  OF  THE  THREE  TUPLE  p<,Y^  WHICH  DESCRIBES 

0         THE  COORDINATES  THAT  WERE  SIMULATED  BY  THE  ROBOT  MOTION. 

SUBROUTINE  SPCAR 

INTEGER  I.J.K.L 

DIMENSION  P0S(1 000.3),SPPOS(1 000.3) 
0         INPUT  CARTESIAN  PATH 

REWIND  15 

READ(15,*)J 

DO  303  1=1  .J-1 

READ(1 5,*)(SPP0S(I,L),L=1 ,3) 
303  CO(^INUE 

C         CALCULATIONS  FROM  SPHERICAL  TO  CARTESIAN 

DO  403  1=1, J-1 
0         CALCULATE/ 

R=SPPOS(M  )*SIN(SPP0S(I,3)) 

POS{l,1)=.5+R*COS(SPPOS(l,2)) 
0         CALCULATE  Y 

POS(l.2)=.5+R*SIN(SPPOS(l,2)) 
0  CALCULATE! 

POS(l,3)=.5+SPPOS(l.1)*COS(SPPOS(l,3)) 
403  CONTINUE 

0         PRINT  AND  I/O 

REWIND  16 

WRITE(9,*)  'SPHERICAL  TO  CARTESIAN  (R,T,P,X.Y,Z)' 

WRITE(16.*)J 

DO  503  1=1  ,J-1 

WRITE  (16,*)(POS(I.L).L=1,3) 
503  CONTINUE 

742  FORMAT  ('  •,T2,I4,2X,6{F5.3,2X)) 

RETURN 
END 
0 

Q  *•«••*•**«*•«****««••*-*•*•.• 

C         *  SCALE  IN  CARTESIAN  COORDNATES  * 

C 

0  THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  CARTESIAN 

C  COORDINATE  THREE  TUPLE  AND  SCALE  SO  THAT  THE  LARGEST  VALUE 

0  IS  ON  THE  SURFACE  OF  A  (1 ,1 ,1 )  CUBE.  THIS  INSURES  THAT  THE 

C  MAXIMUM  ATTEMPTED  MOVEMENT  IS  WITHIN  THE  RANGE  OF  THE  MOTION 
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C         OF  THE  ROBOT  ARM  WHEN  THIS  "NORMALIZED"  VALUE  IS  USED  WITH 
C         A  SCAUNG  FACTOR  (SF).  WITH  SF  LESS  THAN  OR  EQUAL  TO  ONE  THEN 
C         THE  MOTION  WILL  BE  WITHIN  THE  UNIT  CUBE  A  SF  GREATER  THAN 
C         ONE  CAN  BE  USED  WITH  A  CARTESIAN  ROBOT  ARM  WHICH  REQUIRES 
C         INPUTS  TO  BE  SCALED  TO  THE  ARM  LENGTHS.  WHEN  USED  IN 
C         CONJUNCTION  WITH  THE  SPHERICAL  PROGRAM  THE  CARTESIAN  COORD 
C         MUST  BE  WITHIN  THE  UNIT  CUBE. 

SUBROUTINE  CARSC(SF,MAX) 

INTEGER  IJ.K.L 

REALSF,MAX 

DIMENSION  P0S(1 000,3) 

REWIND  2 
C         INPUT  CARTESIAN  PATH 

READ(2,*)  J 

DO305l=1.J-1 

READ(2.*)(POS(I.L).L=1.3) 

IF  (POS(I.I).GT.MAX)  THEN 

MAX=P0S(I.1) 

ENDIF 

IF  (P0S(I,2).GT.MAX)  THEN 

MAX=P0S(I,2) 

ENDIF 

IF  (P0S(I,3).GT.MAX)  THEN 

MAX=P0S(I.3) 

ENDIF 
305  CONTINUE 

C         SCALE 

IF  (MAX.EQ.0.0)  THEN 

GOTO  455 

ENDIF 

DO405l=1,J-1 

P0S(I,1)=P0S(I,1)*SF/MAX 

POS(l,2)=POS(l,2)*SF/MAX 

POS(l.3)=POS{l,3)*SF/MAX 
405  COm"INUE 

C         PRINT  AND  I/O 

REWIND  02 

WRITE  (9,*)  'CARTESIAN  SCALING  SF=',SF 

WRITE  (2.*)  J 

DO  505  1=1  ,J-1 

WRITE(2,*)(POS(l,L).L=1,3) 

WRITE(9.743)  l,(P0S(I.L).L=1 ,3) 

505  corvrriNUE 

743  FORMATC  •,T2,I4,2X,3(F5.3,2X)) 

455  RETURN 

END 
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c 

Q  ****... .^.. ...*.. 

C         •  SCALE  IN  SPHERICAL  COORDNATES  * 

0 

0         THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  SPHERICAL 

0         COORDINATE  THREE  TUPLE  AND  SCALE  RHO  SO  THAT  THE  LARGEST  VALUE 

0         IS  ON  THE  SURFACE  OF  A  UNIT  SPHERE.  THIS  INSURES  THAT  THE 

0  MAXIMUM  ATTEMPTED  MOVEMENT  IS  WITHIN  THE  RANGE  OF  THE  MOTION 

0         OF  THE  ROBOT  ARM  WHEN  THIS  "NORMALIZED"  VALUE  IS  USED  WITH 

0         A  SCAUNG  FACTOR  (SF).  WITH  SF  LESS  THAN  OR  EQUAL  TO  ONE  THEN 

0         THE  MOTION  WILL  BE  WITHIN  THE  UNIT  SPHERE.  A  SF  GREATER  THAN 

C         ONE  CAN  BE  USED  WITH  THE  ARTICULATED  ROBOT  ARM  WHICH  REQUIRES 

0  INPUTS  TO  BE  SCALED  TO  THE  ARM  LENGTHS.  WHEN  USED  IN 

0         CONJUNCTION  WITH  THE  ARTICULATED  PROGRAM  THE  SHPERICAL  COORD 

0         MUST  BE  WITHIN  THE  UNIT  SPHERE.  A  UNIT  SPHERE  IS  DEFINED  TO 

C  HAVE  A  DIAMETER  OF  1  UNIT. 

SUBROUTINE  SPHSC(SF,MAX) 

INTEGER  l,J 

REALSF.MAX 

DIMENSION  SPP0S(1 000,3) 

REWIND  5 
0         INPUT  SPHERICAL  PATH 

READ(5,*)  J 

DO  306  1=1  .J-1 

READ(5.*)(SPP0S(I,L),L=1 ,3) 

IF  (SPP0S(I,1).GT.MAX)  THEN 

MAX=SPP0S(I,1) 

ENDIF 
306  C0lvn"INUE 

C         SCALE 

IF  (MAX.EQ.0.0)  THEN 

GOTO  456 

ENDIF 
DO  406  1=1  ,J-1 

SPP0S(I,1  )=SPPOS(M  )*SF/(2*MAX) 
406  COf^lNUE 

C         PRINT  AND  I/O 

REWIND  05 

WRITE  (9,*)  'SPHERICAL  SCALING  SF=',SF,'  MAX  =',  MAX 

WRITE  (5,*)  J 

DO  506  1=1  ,J-1 

WRITE(5.*)(SPP0S(I,L),L=1 .3) 
506  COJ^INUE 

743  FORMATC  •.T2,I4,2X,3(F5.3,2X)) 

456  RETURN 
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END 


C 

c 


UNSCALE  IN  SPHERICAL  COORDNATES  * 


«****«***«*•******•****•**«****« 


C 

C         THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  SPHERICAL 

0         COORDINATE  THREE  TUPLE  AND  UNSCALE  RHO  SO  THAT  THE  VALUES  ARE 

0         CLOSE  TO  WHAT  THEY  WERE  PRIOR  TO  SENDING  THEM  TO  THE  ROBOT 

0         ARM.  THIS  WAY  THE  FINAL  RESULT  IN  CARTESIAN  WILL  COMPARE  WITH 

0         THE  VALUES  THAT  THEY  STARTED  WITH.  THIS  IS  DONE  BY  USING 

0         A  SCAUNG  FACTOR  (SF). 

SUBROUTINE  USPSC(SF) 

INTEGER  l,J 

REALSF 

DIMENSION  SPP0S(1 000,3) 

REWIND  15 
0         INPUT  SPHERICAL  PATH 

READ(15,*)J 

DO  306  1=1  ,J-1 

READ(15,*)(SPPOS(l,L),L=1,3) 


306 

CONTINUE 

0 

SCALE 

DO  406  1=1  .J-1 

SPPOS(IJ  )=SPP0S(I,1  )*SF 

406 

COI^INUE 

0 

PRINT  AND  I/O 

REWIND  15 

WRITE  (9,*)  'SPHERICAL  UN-SCALING  SF= 

•.SF 

WRITE  (15,*)  J 

DO  506  1=1, J-1 

WRITE(15,*)(SPPOS(l,L).L=1,3) 

506 

CONTINUE 

743 

FORMATC  •,T2,I4.2X,3(F5.3,2X)) 

456 

RETURN 

END 
C 

Q  ••**••*••«•«•*••*««*«•*•*««••* 

0         *  LIMIT  IN  CARTESIAN  COORDNATES  * 

0 

0  THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  CARTESIAN 

0  COORDINATE  THREE  TUPLE  AND  UMIT  VALUES  LARGER  THAN  ONE  SO  THEY 

0  ARE  ON  THE  SURFACE  OF  A  (1 ,1 .1 )  CUBE.  THIS  INSURES  THAT  THE 

0  MAXIMUM  ATTEMPTED  MOVEMENT  IS  WITHIN  THE  RANGE  OF  THE  MOTION 

0  OF  THE  ROBOT  ARM. 
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SUBROUTINE  CARU 
INTEGER  I.J 

DIMENSION  P0S(1 000,3) 
REWIND  2 
C  INPUT  CARTESIAN  PATH 

READ(2/)  J 
DO  307  1=1  ,J-1 
READ(2,*)(POS(I.L).L=1.3) 
IF(POS(I.1).GT.1.0)THEN 

POS(I,1H.O 
ENDIF 
IF(POS(I,2).GT.1.0)THEN 

POS{I.2)=1.0 
ENDIF 
IF(POS(I,3).GT.1.0)THEN 

POS(I,3)=1.0 
ENDIF 
307  CONTINUE 

0         PRINT  AND  I/O 

REWIND  02 

WRITE  (9,*)  'CARTESIAN  LIMITING' 
WRITE  (2,*)  J 
DO  507  1=1  ,J-1 
WRITE(2,*)(POS(I,L),L=1.3) 
507  CONTINUE 

743  FORMATC  ',T2,I4,2X,3(F5.3,2X)) 

RETURN 
END 

C 

f^         *«*••••**••*•**•*•*********** 

0         •  UMIT  IN  SPHERICAL  COORDNATES* 

0 

0         THE  PURPOSE  OF  THIS  PROGRAM  IS  TO  TAKE  A  SET  OF  SPHERICAL 

0         COORDINATE  THREE  TUPLE  AND  UMIT  RHO  SO  THAT  THE  LARGEST  VALUE 

C         IS  ON  THE  SURFACE  OF  A  UNIT  SPHERE.   THIS  INSURES  THAT  THE 

C         MAXIMUM  ATTEMPTED  MOVEMENT  IS  WITHIN  THE  RANGE  OF  THE  MOTION 

C         OF  THE  ROBOT  ARM. 

SUBROUTINE  SPHLI 

INTEGER  l,J 

DIMENSION  SPP0S(1 000,3) 

REWIND  5 
C         INPUT  SPHERICAL  PATH 

READ(5,*)  J 

DO308l=1,J-1 

READ(5,*)(SPP0S(I,L),L=1 ,3) 
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IF  (SPPOS(I.1).GT.0.5)  THEN 

SPPOS(I,1)=0.5 
ENDIF 
308  CCKHNUE 

C         PRINT  AND  I/O 

REWIND  05 

WRITE  (9,*)  'SPHERICAL  LIMITING' 
WRITE  (5.*)  J 
DO  508  1=1  ,J-1 

WRITE(5.*){SPP0S(I,L),L=1 ,3) 
508  CONTINUE 

743  FORMATC  ',T2.I4,2X,3(F5.3,2X)) 

RETURN 
END 
ECF: 


304 


APPENDIX  H 
MISCILLANEOUS  PLOTTINGS 

This  appendix  contains  miscillaneous  plots  that  were  not  included  in 
the  text  but  may  be  of  interest  to  the  reader. 


305 


(S3HDNIJ  Zd 


(S3H3Nli  23 


(53M3NIJ  O 


in 


r 


« 

Z 

V- 

O 

^ 

"^ 

tn 

f- 

Ui 

o 

X 
o  — 

UJ 

-) 
o 
a: 

Ql. 

•o 

to 

c 
o 

X 

o 

i  ) 

t_» 

UJ 

J= 

n> 

z 

iL. 

a> 

o 

o 

—i 

o 

0) 

c 

■♦— ' 

<o 

*f^ 

to 

o 

Q_ 

Q_ 

rvi 

>- 

c 

•o 

o 

c 

4— • 

o 

o 

flO 

^ 

^ 

fVI 

•7~ 

>c 

(SB^^DNl)  13 


153H3NI)  X3 


1S3M3NM  X3 


UJ 


h-  J3 


CO 


to 

CO 

d 

Q- 

o 

!^ 

>/■ 

lO 

Hi 

■" 

CL' 

o 

in 

•  ^— 

^ 

LJ 

..«— 

h- 

^- 

r>.j 

-r 

■o 

iX>  lO 


CTi 


«D 


306 


o 

o 

o          ^ 

-■^ 

V 

_      • 

^""T^^ 

o 

an 

\ 

mm. 

/ 

\       N 

^^         ^* 

jn 

\ 

\n 

/ 

\       ^ 

\ 

\rt     y- 

UJ 

X 

\ 

* 

/ 

\ 

\ 

"t     [iJ 

^)L 

\ 

-^ 

/ 

\ 

\ 

u.^  5 

\ 

'Ow 

\ 

"o  ^ 

\ 

\ 

^ 

\ 

X 

\ 

\ 

/ 

X 

\ 

\ 

u 

\ 

CI 

\ 

y 

K 

U      UJ 

\ 

o 

> 

o 

^     ^ 

\ 

es           _j 

u 

u 

) 

o'^ 

u 

ih 

o    ° 

u 

^ 

J  °     -^ 

« 

c 

« 

« 

• 

« 

• 

• 

• 

o 

C3 

o 

^ 

V 

o 

o 

(S3H3NU 

23 

o 

lS3H3Ni) 

23 

o 

153H3NI) 

;3 

o 

/ 

/ 

"•<« 

/ 

/ 

"^ 

^ 

"^ 

/ 

<NJ 

/ 

(VJ 

/ 

(\J 

V 

N 

'rn 

c 

UJ 

•w  1/1 

J 

••to 

OJ  UJ 

/^ 

.     •  LJ 

fSt  UJ 

S- 

tn 

in 

\ 

TJh      O. 

^^ 

*" 

\ 

UJ 

UJ 

UJ 

Uj        OL 

r 

n 

} 

x: 

u) — 

^ 

tX)  — 

y 

Ifl  ~         UJ 

.  .  ►- 

c 

.    .  ^- 

^ 

CO 

\ 

oo 

(^ 

oo 

• 

_       • 

c 

• 

\ 

o 

o 

\ 

"o 

^ 

\ 

o 

\ 

o 

N 

o 

1 
o 

J. 

G 

'o 

L 

j, 

C 

'"o 

J> 

c 

~o 

• 

• 

• 

• 

• 

* 

• 

t 

■ 

o 

o 

o 

o 

o 

a 

(S3H3NN 

Z3 

IS3M3NI) 

A3 

IS3H0NU 

"X3 

to 

<^ 

o 

■o 

4>^ 

o 

CD 

z: 

O 

(— 

D_ 

O 

CD 

•^ 

C 

<a 

to 
o 

a. 

CL 

t^i 

o     ^ 

5 


o 


IVI 


X 


< 

JU 

QL 

■ — ■ 

^ 

<i> 

rn 

^— * 

o 

Cl 

L. 

frj 

ID 

<u 

— 

ZaL 

^H* 

'_! 

OJ 

— 

£ 

f     1 

•— 

1— 

1:^4 

r--j 

± 

Oj      iTD 


CO 


307 


-« 


fM 


in 


TmTTag3rT3Q 


nNTTyyTToTrSoa 


((NI)^H3cOU    130 


^  8f 


M 


a. 
>- 


^"i; — I" 


X 


CO  «/)  'V  fn 


CJ 


w 


(S3H0NU    X3 


•o 

o 

</i 

J= 

L_ 

•«— > 

O 

Oi 

1 

z: 

LU 

=r« 

=C 

.^M 

^— ' 

o 

iT3 

o 

Q. 

^■^ 

OJ 

rvi 

> 

T3 

^ 

s^ 

c 

<D 

o 

^ 

•^^ 

->- 

■^-^ 

ID 

X 

■•-' 

TD 

o 

e 

X 

(O 

UJ 

c 

Q_ 

o 

LU 

•  •*— 

I— 

(_J 

U1 

OJ 

^-^ 

n 

■o 

1 

cu 

fL 

<■_> 

O) 

TD 

c 

Oi 

CD 

^ 

Q_ 

^ 

:>- 

^H* 

X 

itD 

, 

CL 

C! 

•^^ 

l_ 

1 

CJ 

P*"* 

^^B 

3 

^— 

<-J 

;i 

1. 

£= 

•^^ 

LJ 

O 

o 

V"-. 

^^ 

iD 

-•aX 

II 

K) 

<— 

T 

•  »■• 

f^ 

a> 

k_ 

~ 

i)0 

CD 

--> 

308 


TTffrrasirra 


mTTTiaTTTrrrTn 


((Ni)8ii3cOU    tOQ 


ETi; — r 


ID       in       V       (*> 


-Z — T 


rw 


(S3M3NI]   O 


■o 

o 

. 

1— 

f/) 

4-* 

u. 

HI 

o 

L- 

^_ 

U 

IT« 

LU 

•^ 

SZ 

•^ 

■*-> 

O 

CD 
Q- 

<X> 
> 

r-i 

•VD 

C 

c 

CO 


■o 


f  "l 

»_j 

y 

iZZ 

UJ 

«o 

Q_ 

LU 

o 

f- 

*— > 

cn 

o 

Oi 

■o 

o 

rt) 

!_ 

o 

Q_ 

XD 

CD 

CO 

-— ^ 

0- 

/— 

—J 

z;^ 

d 

•^'" 

Q- 

L- 

•^— 

«o 

1 

^^ 

•^■" 

— ^ 

^_ 

l_l 

.y.. 

t. 

f~ 

._ 

c: 

LJ 

o 

_,_^ 

u-» 

i=i 

II 

-..^ 

K) 

c 

X 

Oj 

^— 

(D 

13 

-> 

cr. 

309 


310 


311 


TnTrrasiTTM 


5" ST 

rniTTaBmm-331 


F ^ 

a 


£    3;     J" 


X 


ir~ifr 


t\i    *- 


-.       CJ 

i  ^ 

>- 

X 
(Si 


■^ 

>- 

in 

^ 

X 

f ) 

"O 

X 

c 

LU 

00 

CL 

UJ 

0 

h- 

.«-« 

CO 

0 

CI) 

^'""•. 

"CI 

0 

0) 

L- 

0 

CL 

<D 

-— - 

Q- 

f^ 

^^ 

ID 

X. 

Q. 

^ 

l_ 

•  »— 

ID 

II 

»^— 

*r^ 

••■^ 

0 

-^— 

p: 

LJ 

.— 

0 

'K 

II 

^w* 

r*"^ 

c 

X 

■^ 

0^ 

g          !■ 

1. 

ID 

3 

--^ 

cr,' 

' 

(53MaNI)    AO 


312 


<s< 


n 


o  u^ 


-S5 


TTWTTSairOJ 


((Nhd^rOl)  Zyi 


((ND^^tOi)   IX 


b      o)       <b       r-       «o^     «n       ^      «•»       «n*       »- 


a  -J 

a. 
>- 

X 


(>j 


fS3H0Nl)    X3 


T3 

O 

CO 

-C 

L. 

■<— » 

O 

"X» 

1 

z: 

LU 

^1 

J= 

_ 

«^ 

o 

CO 

o 

CL 

Oi 

rvl 

> 

"O 

^ 

c 

c 

lO 

o 

;^ 

:>- 

<D 

X 

•—> 

■a 

o 

c 

X 

ID 

UJ 

a. 

O 

llj 

•  »•— 

\— 

(J 

W) 

(D 

^-C 

O 

■o 

L_ 

CD 

Q_ 

O 

<D 

"O 

r^ 

Oi 

<o 

Q:: 

a. 

JC 

;>- 

<^<^ 

>c 

<T3 

■l 

Q_ 

c 

(D    •.— 


O 
o^  > 


313 


ao 


<A 


M 


(S« 


TTTmroiroi 


F 


rmn^smjiTTSQ 


^r~3* — J- 


J>     J>     4 


TTT 


rrmisiTTrrrTso 


t2 


<SI 


>- 

X 


(S3H3NI)    O 


o  — 
o 

— '    II 

X    - 


314 


u> 


■+-0 


?- 


0»- 


(\i 


«0 


*^l-» 
-W 


rsi 


TTRmgssrrJD 


rnmgsTrffiT73Q 


rTFrrrasmrnTJO 


E"^ 


X 


1 — 3r 


—\ r- 


(N4 


a. 

X 


(S3M3NI)   A3 


UJ 


£ 


TO 

o 


ID 


CO 

O 


— ■  CO 

>  1 

.2  ^-'^ 

0  i 


Q_ 
UJ 

I— 


CJ  O) 

"O  CO 

03  , — 

I  a.  - 


lO 


1 


G 

in 

o 

JZ 

o 

■- r" 

f-) 

II 

X 

c: 

<Xi 

a 

u^ 

^^mm 

"^ 

CO 

CO 

> 

315 


r 


IC  *  JO 

rTRTraGmrrrCT        rwrisinrrrTJo        rrrmOTrnnrrno 


rS3H3NI}    12 


ID 

O 

a 

.4— • 

0 

CD 

*— 

UJ 

ZD-f 

sz 

^^ 

0 
0 

Ol 

r»j 

> 

TD 

C 

c 

DO 

0 

^ 

>- 

.f— 1 

(D 

X 

f.l 

X 

ID 

UJ 

e 

CL 

0 

LU 

4_/ 

I— 

u 

CO 

CD 

^^ 

0 

■0 

^ 

CD 

UL 

U 

0) 

CD 

ck: 

a. 

-^j-- 

>- 

" 

X 

1^ 

^— * 

c: 

03 

■*— 

Q- 

1 

X 

^]_ 

.^MB 

•  CSX 

pv> 

,— 

IXi 

C 

3: 

0 

^___ 

0 

tD 

in 

'-Mir*" 

rr 

II 

X 

c 

iXi 

c 

*^ 

p^H 

:^ 

■JO 

en 

~^. 

316 


317 


rT!m^83T7rroi 


nNTTiSTTolTTSQ 


((NDiMJJfi-Ol)    IX 


5  CL 

OLi  (o 

Cki  — 

^'  Q_ 

a,    I 


o 

in 

II 


en  --' 


(S3M3NN    A3 


318 


Sitf 


t 


^^ 


-8 


I 


<NJ 


i 


^ 


^    x 


s^ 


£ 


rmnsaTrBrrrrn 


nrrrrasTTTO-zro 


0. 

>- 


»n       •^       «'>       «\j       ^      o 


7T3h3nTTT5 


TD 

O 

CO 

CD 

o 

=r> 

LU 

I—* 

sz 

'^~ 

^^ 

(_i 

(O 

o 

Ql 

cu 

r--ji 

"»- 

TD 

(— 

C 

o 

CO 

.^^ 

<^-^ 

>- 

■o 

o 
>: 

UJ 

■o 

CO 

Q- 

o 

UJ 

h- 

*-> 

fO 

o 

O) 

■o 

o 

o 

Ql 

rj 

a> 

"O 

c~ 

ai 

(O 

Ck: 

a. 

^ 

■>- 

X 

"T" 

«_> 

.— 

<D 

._ 

a. 

1 

X 

»— 

• 

E 

^ 

o 

^. 

— 

■o 

II 

rr 

c 

■  *^ 

u^ 

r^ 

OJ 

j*^ 

!_ 

ro 

CJ) 


319 


rmTTSsrnmT3Q 


((Nl)^Jf.Oli  S3Q 


nrnrsarnrn-TX 


,1^ 

¥ 

■o 

o 

(  > 

^^ 

O     rX 

^\>i 

j^ 

5  L' 

M 

LU 

a.    P 

O 

H   c 

•" 

X 

rn"^ 

e 
e 

V 

£ 

■3  ^ 

O      Q_ 

►- 

a 
a 

lalion, 
X,Y,  an 

o 

'duced),  STEP  Exci 
ne  Projection  and 

(S3M3NU    AD 


en    S 


j    -D    - 
X   — 


^    I/. 
— .   O 


320 


i" 


^^ 


nm7B3jnrrroi 


nNi)^3t.0k)  230 


(IMI)^3t-0t)   130 


(S3H0NI)    A3 


•o 


z: 


O     Q_ 

Hi   rvj 


<D 


X 


lO     — 

"oj   E 


o 
II 


321 


(S4 


t-     i s 


«o< 


'{>< 


<--      PC 


t\* 


M 


rmrTOTTnrrroj 


rTFnTBamrrrm 


&■ 


(mil^3t.0n  i30 


.^s 


0. 
X 


T53fl3?nTT3 


a: 


O      CO 

—f     o 


■^5 

o     '^ 
O    CI. 

>  -o 

,  cr 

c  CO 
o 

^   ^^ 

.■^  -o 
o    c 

X     Id 

l^      r- 

Q-     O 
UJ    - 

•^    o 
CD    a» 

— C"   o 

0U>     Q_ 
(U      'CI 


a. 

5 


1^. 


un 


CD  > 


322 


LIST  OF  REFERENCES 

Brady,  M.,  and  others,  Robot  Motion:  Planning  and  Control.  MIT  Press,  1982. 

Coiffet.P..  Robot  Technology:  Modeling  and  Control.  Prentice-Hall,  Inc,  1983. 

Featherstone,  R.,  "Position  and  Velocity  Transformations  Between  Robot 
End-Effector  Coordinates  and  Joint  Angles,"  International  Journal  of 
Robotics  Research,  v.  2,  no.  2,  Summer  1983. 

Fu,  K.S.,  Gonzalez,  R.C.,  and  Lee,  C.S.G.,  Robotics:Control.  Sensing.  Vision. 
and  Intelligence.  McGraw-Hill  Book  Co.,  1987. 

Kalogiros,  G.,  Automatic  Control  of  Robot  Motion  ,  Masters  Thesis,  Naval 
Postgraduate  School,  Monterey,  CA,  December  1987. 

Koran,  Y.,  Robotics  for  Engineers.  McGraw-Hill  Book  Co.,  1985. 

Lee,  C.S.G.,  "Robot  Arm  Kinematics,  Dynamics,  and  Control."  Computer. 
December  1 982. 

Lee,  C.S.G.,  and  Chang,  P.R.,  Efficient  Parallel  Algorithm  for  Robot  Inverse 
Dynamics  Computation,  paper  presented  at  the  Procedings  of  1986 
International  Conference  on  Robotics  and  Automation,  April,  San  Fransisco, 
CA. 

Lee,  C.S.G.,  and  Ziegler,  M.,  "Geometric  Approach  in  Solving  Inverse 
Kinematics  of  PUMA  Robots,"  IEEE  Transactions  on  Aerospace  and  Electrical 
Svstems.  v.  AES-20,  no.  6,  November  1984. 

Loranzo-Perez,  T.,  "Robot  Programming,"  Proceedings  of  IEEE,  v.  71 ,  no.  7, 
pp.  821-841,  July  1983. 

Ozaslan,  K.,  The  Near  Minimum  Time  Control  of  a  Robot  Arm.  Masters  Thesis, 
Naval  Postgraduate  School,  Monterey  CA,  December  1986. 


323 


Paul,  R.P.,  Robot  Manipulators.  Mathmatics.  Proarammina.  and  Control ,  MIT 
Press,  1981. 

Thaler,  G  J.  and  Stein,  W.A.,  "Transfer  Function  and  Parameter  Evaluation  for 
D-C  Servomotors,"  Applications  and  Industn/.  Januan/  1956. 


324 


BIBLIOGRAPHY 

Ardayfio,  D.D.,  and  Pottinger,  H.J.,  Computer  Control  of  Robotic 
Manipulators,  Mech.  Eng..  v.  104,  no.  8  August  1982,  pp.  40-45. 

Asada,  H.,  and  Slotine,  J.J.,  Robot  Analysis  and  Control.  John  Wiley  and  Sons, 
1986. 

Craig,  J.T.,  Introduction  to  Robotics:  Mechanics  and  Control. 
Addison-Wesley,  1986. 

Denavit,  J.,  and  Hartenberg,  R.S.,  A  Kinematic  Notation  For  Lower  Pair 
Mechanisms  Based  on  Matrices,  J.  Appl.  Mech.  ASME.  June  1955,  pp. 
215-221. 

Derby,  S.  Simulating  Motion  Elements  of  General  Purpose  Robot  Arms, 
International  Journal  of  Robotics,  v.  2,  no.  1  pp.  3-12, 1983. 

Ersu,  E.,  and  Nungesser,  D.,  A  Numerical  Solution  of  the  General  Kinematic 
Problem.  Technical  Institute  of  Darnstadt,  FRG. 

Grossman,  P.P..  Programming  a  Computer  Controlled  Manipulator  by  Guiding 
Through  The  Motions.  IBM  J.T.  Watson  Res.  Cen.,  Res.  Rep  RC6393,  1977 
(declassified  1981). 

Holm,  R.E.,  Computer  Path  Control  of  an  IR,  Proc.  of  8th  ISIR.  1978,  pp. 
327-332. 

Luh,  J.Y.S.,  Walker,  M.W.,  and  Paul,  R.P.,  On-Line  Computational  Scheme  For 
Mechanical  Manipulators,  J.  Pyn.  Syst.  Meas.  Control,  v.  102, 1980a,  pp. 
69-76. 

Paul,  R.P.,  Modelling.  Trajectory  Calculation  and  Servoing  of  a  Computer 
Control  Arm.  Stanford  University,  Al  Lab,  Memo  AIM  177,  1972. 


325 


Paul,  R.P.,  Manipulator  Cartesian  Path  Control,  IEEE  Trans.  SYST.  Man 
Cybernet..  SMC-9,  1979,  pp.702-711. 

Suh,  C.H.  and  Radcliffe,  C.W.,  Kinenriatics  and  Machanisms  Design.  John  Wiley 
andSons,  New  York,  1978. 

Takase,  K.,  Paul,  R.P.,  and  Berg,  E=E.,  A  Structured  Approach  To  Robot 
Programming  and  Teaching,  paper  presented  at  IEEE  COMPSAC,  Chicago,  II, 
November  1979. 

Tasi,  L,  and  Morgen,  A.P.,  Solving  the  Kinematics  of  Most  General  6  and  5 
Degree  of  Freedom,  paper  presented  at  ASME  Mechanism  Conference, 
Boston,  7-10  October  1984,  paper  84-DET-20. 

Taylor,  R.H.,  Planning  and  Execution  of  Straight  Line  Manipulator 
Trajectories.  IBM  J.  Res.  Dev.,  v.  23,  no.  4, 1979,  pp.  424-436. 

Whitney,  D.E.,  The  Mathmatics  and  Coordinate  Control  of  Prosthetic  Arms 
and  Manipulators.  J.  Dyn  Svst.  Meas.  Control.  December  1972,  pp.  303-309. 


326 


INITIAL  DISTRIBUTION  LIST 

No.  Copies 

1.  Defense  Technical  Information  Center  2 
Cameron  Station 

Alexandria,  Virginia  22304-6145 

2.  Library,  Code  0142  2 
Naval  Postgraduate  School 

Monterey,  California  93943-5002 

3.  Department  Chairman,  Code  62  1 
Department  of  Electrical  and  Computer  Engineering 

Naval  Postgraduate  School 
Monterey,  California  93943 

4.  Professor  G.J.  Thaler,  Code  62Tr  5 
Department  of  Electrical  Engineering 

Naval  Postgraduate  School 
Monterey,  California  93943 

5.  Professor  H.A.  Titus,  Code  62Ti  1 
Department  of  Electrical  Engineering 

Naval  Postgraduate  School 
Monterey,  California  93943 

6.  Lieutenant  Steven  G.  Goodway  5 
c/o  Bill  Goodway 

5310SEOetkin  Road 
Milwaukie,  Oregon  97267 

7.  Professor  D.L.  Smith,  Code  69Sm  1 
Department  of  Mechanical  Engineering 

Naval  Postgraduate  School 
Monterey,  California  93943 


327 


8.  Commander 

Naval  Sea  Systems  Command 
Washington,  D.C.  20361-0001 

9.  Commander 

Naval  Surface  Weapons  Center  White  Oak 
Silver  Springs,  Maryland  20910 

10.  Director 

Naval  Research  Labotatory 
Washington,  D.C.  20375 

1 1 .  Commander 

Naval  Weapons  Center 
China  Lake,  California  93555 

1 2.  Chief  of  Naval  Operations 
Attn:  Code  OP-03 
Washington,  D.C.  20350 

1 3.  Chief  of  Naval  Operations 
Attn:  Code  OP=04 
Washington.  D.C.  20350 

14.  Dr.  Russ  Werneth 

Naval  Surface  Weapons  Center 

G-40 

Silver  Springs,  Maryland  20903-5000 

15.  Mr.  W,  Butler 

Naval  Sea  Systems  Command 
Attn:  C90G 
Washington,  D.C.  20362 

328 


3^ 


^^ 


r 
1.. 

MOi' 


HOOL 


^ohot 


r 


7 


Thesis 

G571    Goodway 

c.l       Path  followir.g  robot 


/i/'"""% 

%. 


thesG571 

Path  following  robot. 


3  2768  000  77471  5 

DUDLEY  KNOX  LIBRARY 


.iil-i.liUiJilli.:.. 


iLJ-lLJ-JJll'illlijli 


